pv  R|'nMj^'''N  test  chart 


jnt.BU.Cl0 

Technical  Report  982 

Error  Detection 
and  Recovery  for 
Robot  Motion 
Planning  with 
Uncertainty 


ThU  dooimont  has  boen  app'cvrd 
toi  public  leleose  and  aoie,  its 
distribution  is  ucilimited. 

Bruce  R.  Donald! 


MIT  Artificial  Intelligence  Laboratory 


*AGC  0«i« 


REPORT  DOCUMENTATION  PAGE 


'  ^£oo"T  MuMae* 

AI-TR-982 


*  Title  rand  £«*><»•> 


ERROR  DETECTION  AND  RECOVERY  FOR  ROBOT 
MOTION  PLANNING  WITH  UNCERTAINTY 


*VJ  T  MO  nr  a; 


Bruce  R.  Donald 


■EnroMMiNC  OnoaNIZATiON  M  kMC  ANO  AOOnctS 

Artificial  Intelligence  Laboratory 
545  Technology  Square 
Cambridge.  MA  02139 


<1.  contkoluing  omcE  name  ano  aoomcss 
Advanced  Research  Projects  Agency 
1400  Wilson  Blvd. 

Arlington,  VA  22209 


14  MONIToniNS  ASEnCY  name  «  AOONCSVK  dlflaranl  i 

Office  of  Naval  Research 
Information  Systems 
Arlington,  VA  22217 


IE  OtSTAiauTION  STATEMENT  (at  ihf  NapaatJ 


READ  INSTRUCTIONS 
BEFORE  COMPLETING  FORM 


s.  necinient's  catalog  mumbem 


I-  TV»C  ntPOMT  A  PCMOO  COVCACO 

Technical  Report 


i.  ^lAronMiMO  omQ,  ntPonr  NUMeen 


OMTAACT  op  OMANt 

N00014-81-K-0494 

-85-K-0124 


!:f(cni]cicci 


le.  nnoONAM  element,  nnoject.  task 

ANEA  a  >ONK  UNIT  NUMBENS 


II.  NERORT  DATE 

July  1987 

IS.  NUMBER  or  RAOeS 

_ 310 


caNtMiiMa  out—)  IB.  security  Class.  uma 

UNCLASSIFIED 


Distribution  is  unlimited. 


t7.  OISTPlieuTlON  STATtMCNt  Cl  tfC  AActfACt  tn  9C€k  3P,  If  AlflArAni  Pmm  MwpmH) 


ft.  KEY  90^0$  ICAfiflMVA  An  fAWAA  Al^A  If  AAAAAAAPT  MaaA  mmiPm) 

Robotics,  motion  planning,  uncertainty.  Error  Detection  and  Recovery,  Error 
Diagnosis,  geometric  uncertainty,  computational  geometry,  geometric  reasoning, 
planning  with  uncertainty,  model  error,  model  uncertainty,  EDR,  failure  mode 
analysis 


10  abstract  fCaniNwa  an  ravofa*  alBa  It  iwaaaaafr  t—t*w  Of  SlaaS  Maiaaa> 

Robots  must  plan  and  execute  tasks  in  the  presence  of  uncertainty.  Uncertainty  arises 
from  sensing  errors,  control  errors,  and  uncertainty  in  the  geometry  of  the  enviionment. 

The  last,  which  is  called  model  error,  has  received  little  previous  attention,  ^^e  present  a 
framework  for  computing  motion  strategies  that  are  guaranteed  to  succeed  in  the  presence 
of  all  three  kinds  of  nncertaintv.  The  motion  strategies  comprise  sensor-based  gross  motions, 
compliant  motions,  and  simple  pushing  motions. 

It  is  not  always  possible  to  find  plans  that  are  guaranteed  to  succeed.  For  example,  if 


toirranciiig  errors  render  an  assembly  infeasible,  the  plan  executor  should  stop  and  signal 
failure.  In  such  cases  the  insistence  on  guaranteed  success  is  too  restrictive.  For  this  reason 
we  investigate  Error  Detection  and  Recovery  (EDR)  strategies.  EDR  plans  will  succeed  or 
fail  recognizably;  in  these  more  general  strategies,  there  is  no  possibility  that  the  plan  will 
fail  without  the  executor  realizing  it.  The  EDR  framework  fills  a  gap  when  guaranteed  plans 
cannot  be  found  or  do  not  exist:  it  provides  a  technology  for  constructing  plans  that  might 
work,  but  fail  in  a  "reasonable”  way  when  they  cannot. 

While  EDR  is  largely  motivated  by  the  problems  of  uncertainty  and  model  error,  its 
applicability  may  be  quite  broad.  EDR  has  been  a  persistent  but  ill-defined  theme  in  both 
Al  and  robotics  research.  We  give  a  constructive,  geometric  definition  for  EDR  strategies  and 
show  how  they  can  be  computed.  This  theory  represents  what  is  perhaps  the  first  systematic 
attack  on  the  problem  of  error  detection  and  recovery  based  on  geometric  and  physical 
reasoning.  We  also  describe  an  implemented  planner  in  a  restricted  domain,  and  discuss  the 
applicability  of  traditional  computational  geometry  in  algorithms  for  EDR  planning. 


r  .'1 


iJ. 


.'t 

I 

.S 

t 

I 


Error  Detection  and  Recovery  for 
Robot  Motion  Planning  with  Uncertainty 


})y 

Bruce  Randall  Donald 

B.A.,  Yale  Uni\’ersity 
1980 

S.M..  Electrical  Engineering  and  Computer  Science.  MIT 

1984 

Submitted  to  the 

Department  of  Electrical  Engineering  and  Computer  Science 

in  partial  fulfillment  of  the  requirements  for  the  degree  of 

Doctor  of  Philosophy 

at  the 

Massachusetts  Institute  of  Technology 
July.  1987 

c Massachusetts  Institute  of  Technology.  1987 


Signature  of  Author 


Department  of  Electrical  Engineering  and  Computer  Science 

Jvily.  1987 


.Accepted  by 


.Arthur  C.  Smith 

Chciirman.  Departmental  Committee  on  Graduate  Students 


Error  Detection  and  Recovery 
for  Robot  Motion  Planning  with  Uncertainty 


by 

Bruce  Randall  Donald 


Submitted  to  the 

Depjuiment  of  Electrical  Engineering  and  Computer  Science 
in  partial  fulfillment  of  the  requirements  for  the  degree  of 
^  Doctor  of  Philosophy 


I 


1 


Robots  must  plan  and  execute  tasks  in  the  presence  of  imcertainty.  Uncer¬ 
tainty  arises  from  sensing  errors,  control  errors,  and  imcertainty  in  the  geometry 
of  the  environment.  The  last,  which  is  called  model  error,  has  received  little  pre¬ 
vious  attention.  We  present, a  framework  for  computing  motion  strategies  that  Me 
guaranteed  to  succeed  in  the  presence  of  all  three  kinds  of  uncertainty.  The  mo¬ 
tion  strategies  comprise  sensor-based  gross  motions,  compliant  motions,  and  simple 
pushing  motions. 

It  is  not  always  possible  to  find  plans  that  are  guaranteed  to  succeed.  For  ex¬ 
ample,  if  tolerancing  errors  render  an  assembly  infeasible,  the  plan  executor  should 
stop  and  signal  failure.  In  such  cases  the  insistence  on  guaranteed  success  is  too 
restrictive.  For  this  reason '•we  investigate  Error  Detection  and  Recovery  (EDR) 
strategies/!  EDR  plans  will  succeed  or  fail  recognizably:  in  these  more  general 
strategies,  there  is  no  possibility  that  the  plan  will  fail  without  the  executor  realiz¬ 
ing  it.  The  EDR  framework  fills  a  gap  when  guaranteed  plans  cannot  be  found  or 
do  not  exist:  it  provides  a  technology  for  constructing  plans  that  might  work,  but 
fail  in  a  ‘^reasonable”  way  when  they  cannot.  .  r  ■  '  j  y  ,  j  /  ‘ .  r 

While  EDR  is  largely  motivated  by  the  probleihs  of  uncertainty  and  model  er¬ 
ror,  its  applicability  may  be  quite  broad.  EDR  has  been  a  persistent  but  ill-defined 
theme  in  both  AI  and  robotics  research.  We  give  a  constructive,  geometric  definition 
for  EDR  strategies  and  show  how  they  can  be  computed.  This  theory  represents 
what  is  perhaps  the  first  systematic  attack  on  the  problem  of  error  detection  and 
recovery  based  on  geometric  and  physical  reasoning.  We  also  describe  an  imple¬ 
mented  planner  in  a  restricted  domain,  and  discuss  the  applicability  of  traditional 
computational  geometry  in  algorithms  for  EDR  planning. 

Thesis  Supervisor:  Tomas  Lozano-Perez 
Title;  Associate  Professor  of  Computer  Science 
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1.  Description  of  Proi)leni  and  the  Planner 
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We  show  that  model  error  can  he  represente<J  by  prjsition  uncertainty  in  a 
generalized  configuration  space,  W’e  describe  the  structure  of  this  s'^ace.  and  how 
motion  strategies  may  he  planned  in  it 

It  is  not  always  possible  to  find  phuis  that  are  guaranteed  to  succeeti.  In 
the  presence  of  model  error,  such  plans  may  not  even  exist  For  this  reason  we 
investigate  Error  Detection  and  Recovery  (EDRj  strategies.  W'e  charactenze  such 
strategies  geometrically,  and  propose  a  formal  framework  for  constructing  them. 

This  thesis  offers  two  contributions  to  the  theory  of  manipulation.  The  first 
IS  a  framework  for  planning  motion  strategies  with  model  error.  Model  error  is 
a  fundamental  problem  in  robotics,  and  we  have  tried  to  provide  a  principled, 
lufcise  approach.  The  framework  can  be  described  very  compactly,  although  many 
algorithmic  and  implementational  questions  remain. 

The  second  contribution  is  a  formal,  geometric  approach  to  EDR.  While  EDR  is 
largely  motivated  by  the  problems  of  uncertainty  and  model  error,  its  applicability 
may  be  quite  broad.  EDR  has  been  a  persistent  but  ill-defined  theme  in  both 
A I  and  robotics  research.  Typically,  it  is  viewed  as  a  kind  of  source-to-source 
transformation  on  robot  programs:  for  example,  as  a  method  for  robustifying  them 
by  introducing  sensing  steps  and  conditionals.  We  take  the  view  that  if  one  can 
actually  plan  to  sense  sm  anomalous  event,  and  to  recover  from  it,  then  it  is  not 
an  error  at  all.  When  such  plans  can  be  guaranteed,  they  can  be  generated  by  the 
[LMT]  method.  In  our  view  of  EDR,  an  “error”  occurs  when  the  goal  cannot  be 
recognizably  achieved  given  the  resources  of  the  executive  and  the  state  of  the  world. 
The  EDR  framework  fills  a  gap  when  guaranteed  plans  cannot  be  foimd  or  do  not 
exist:  it  provides  a  technology  for  constructing  plans  that  might  work,  but  fail  in 
a  “reasonable”  way  when  they  cannot.  This  theory  attempts  a  systematic  attack 
on  the  problem  of  error  detection  and  recovery  based  on  geometric  and  physical 
retisoning. 


1.1.  Application  and  Motivation 


1.1.1  A  Simple  Example 


Consider  fig.  1,  which  depicts  a  peg  in  hole  insertion  task.  One  could  imagine 
a  manipulation  strategy  derived  as  follows:  The  initial  plan  is  to  move  the  peg 
straight  down  towards  the  bottom  of  the  hole.  However,  due  to  imcertainty  in  the 
initial  position  of  the  peg,  the  insertion  may  fail  because  the  peg  contacts  to  the  left 
or  right  of  the  hole.  Either  event  might  be  regarded  as  an  “error.”  The  “recovery” 
action  is  to  move  to  the  right  (if  the  peg  contacted  to  the  left)  and  to  move  to  the 
left  (if  the  peg  contacted  to  the  right).  Thus  a  plan  can  be  obtained  by  introducing 
sensing  steps  and  conditional  branches. 

Suppose  that  this  conditional  plan  can  be  guaranteed — that  is,  it  is  a  complete 
meinipulation  strategy  for  this  simple  task.  In  this  case,  it  seems  strange  to  view 
the  contact  conditions  as  “errors.”  We  do  not  regard  these  events  as  “errors.”  Oxir 
re^lsoning  is  that  if  they  can  be  detected  and  planned  for,  then  they  are  simply 
events  in  a  guaranteed  plan. 

We  are  interested  in  a  different  class  of  “errors.”  Now  suppose  that  there  is 
uncertainty  in  the  width  of  the  hole.  If  the  hole  is  too  small,  we  will  consider  this  an 
error,  since  it  causes  all  plans  to  fail.  Similarly,  if  some  object  blocks  the  hole,  and 
cannot  be  pushed  aside,  this  is  also  an  error,  since  it  makes  the  goal  unreachable. 
If  either  error  is  possible,  there  exists  no  guaranteed  plan,  for  there  is  no  assurance 
that  the  task  can  be  accomplished.  Since  no  guaranteed  plan  can  be  found,  we  are 
left  with  the  choice  of  giving  up,  or  of  considering  a  broader  class  of  manipulation 
strategies:  plans  that  might  work,  but  fail  in  an  “reasonable”  way  when  they  cannot. 
Specifically,  we  propose  that  EDR  strategies  should  achieve  the  goal  when  it  exists 
and  is  recognizably  reachable,  and  should  signal  failure  when  it  is  not.  For  example, 
^ln  EDR  strategy  for  the  peg-in-hole  problem  with  model  error  might  attempt  to 
achieve  the  insertion  using  compliant  motion,  but  be  prepared  to  recognize  failure 
in  caise  the  hole  is  too  small.  Below,  we  describe  how  an  implemented  planner, 
called  Limited,  synthesizes  such  strategies.  LIMITED  is  an  implementation  of  the 
EDR  theory  in  a  restricted  domain. 

1.1.2  Application:  Planning  Gear  Meshing 

We  must  stress  that  EDR  is  not  limited  to  problems  with  model  error.  There 
are  many  applications  in  which  the  geometry  of  the  environment  is  precisely  known, 
but  in  which  guaranteed  plans  cannot  be  foimd,  or  are  very  difficult  to  generate. 
We  now  describe  such  a  situation. 

An  interesting  application  domain  for  EDR  is  gear  meshing.  It  is  an  example 
where  EDR  is  applicable  even  though  the  shape  of  the  manipulated  parts  is  precisely 
known.  Let  us  consider  a  simplified  instance  of  this  problem.  In  fig.  2  there  are 


Fig.  1.  The  goal  is  to  insert  the  peg  in  the  hole.  No  rotation  of  the  peg  is 
allowed.  One  can  imagine  a  strategy  which  attempts  to  move  straight  down,  but 
detects  contact  on  the  top  surfaces  of  the  hole  if  they  occur.  If  the  peg  sticks 
on  the  top  surfaces,  the  manipulator  tries  to  move  to  the  left  or  right  to  achieve 
the  hole.  Are  these  contact  conditions  '‘errors”?  We  maintain  that  they  are  not, 
since  they  can  be  planned  for  and  verified. 

two  planar  gear-like  objects,  A  and  B.  The  task  is  to  plan  a  manipulation  strategy 
which  will  mesh  the  gears.  The  state  in  which  the  gears  are  meshed  is  called  the 
goal. 

We  will  consider  two  variants  of  this  problem.  In  the  first,  we  assume  that  the 
manipulator  has  grasped  A,  and  that  neither  A  nor  B  can  rotate.  However,  A  can 
slide  along  the  surfaces  of  B.  In  the  second,  B  is  free  to  rotate  about  its  center, 
but  this  rotation  can  only  be  effected  by  pushing  it  with  A.  In  both  cases,  the 
initial  orientation  of  B  is  unknown.  We  regEU-d  A  as  the  moving  object  and  B  as 
the  environment;  hence  even  though  the  shape  of  B  is  precisely  known,  we  choose 
to  view  the  uncertainty  in  B ’s  orientation  as  a  form  of  model  error.  In  the  first 
case,  the  system  has  only  two  degrees  of  motion  freedom.  In  the  second,  there  are 


three  degrees  of  motion  freedom,  one  of  which  is  rotational,  since  B  can  be  pushed. 
We  distinguish  between  the  rotation  and  non-rotation  variants  of  the  problem  in 
order  to  highlight  the  additional  techniques  our  plamner  employs  when  rotations  aure 
introduced. 


In  both  variations,  there  is  uncertainty  in  control,  so  when  a  motion  direction  is 
commanded,  the  actuad  trajectory  followed  is  only  approximately  in  that  direction. 
There  is  also  uncertainty  in  position  sensing  and  force  sensing,  so  that  the  true 
position  and  reaction  forces  are  only  known  approximately.  The  magnitude  of  these 
uncertainties  are  represented  by  error  balls. 

In  general,  a  commanded  motion  of  A  may  cause  A  to  move  through  free  space, 
and  contact  B,  possibly  causing  B  to  rotate.  Our  EDR  theory  is  a  technique  for 
analyzing  these  outcomes  geometrically  to  generate  strategies  that  achieve  the  goal 
when  it  is  recognizably  reachable,  and  signal  failure  when  it  is  not. 
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In  an  experiment,  the  EDR  theory  in  the  gear  domain  was  applied  using  the 
planner,  LIMITED,  as  follows.  Consider  the  problem  of  meshing  two  planar  gears, 
under  imcertadnty  as  above.  Suppose  that  gear  B  can  rotate  pstssively  but  has 
unknown  initial  orientation,  as  above.  Suppose  that  A  has  been  gripped  by  a 
robot.  The  initial  position  of  A  is  uncertain.  The  robot  can  impart  either  pure 
forces  (translations),  or  pure  torques  (rotations)  to  A.  The  planner  can  choose 
the  direction  of  translation  or  rotation.  Can  a  multi-step  strategy  of  commanded 
translations  and  rotations  be  foimd  to  mesh  the  gears? 

Limited  was  able  to  generate  an  EDR  strategy  for  this  problem.  The  charac¬ 
teristics  of  the  experiment  axe: 

•  There  are  three  degrees  of  motion  freedom  (two  translational  and  one  rota¬ 
tional)  for  A. 

•  There  is  one  degree  of  rotationsJ  model  error  freedom  (the  orientation  of  B). 

•  It  is  possible  to  push  B  to  change  its  orientation. 

•  There  is  sensing  and  control  uncertainty. 

•  The  geometry  of  the  gears  is  complicated — they  have  many  edges. 

•  Quasi-static  analysis  [Mason]  is  used  to  model  the  physics  of  interaction  be¬ 
tween  the  gears. 

Thus  we  have  a  kind  of  four-degree  of  freedom  planning  problem  with  tmcer- 
tainty  and  pushing.  To  generate  multi-step  EDR  strategies  imder  pushing,  LIMITED 
employed  the  EDR  theory  together  with  a  technique  called  failure  mode  analysis. 

Now,  there  may  exist  a  guaranteed  strategy  to  mesh  the  gears.  For  example, 
experimental  evidence  suggests  that  for  involute  teeth  gears,  almost  any  meshing 
strategy  will  succeed.  For  other  gear  shapes  perhaps  some  complicated  translation 
while  spinning  A  will  always  succeed.  I  don’t  know  if  there  is  such  a  guaranteed 
strategy  for  this  case.  It  seems  difficult  for  a  planner  to  synthesize  such  guaranteed 
strategies,  or  even  to  verify  them,  if  they  exist  at  all. 

A  person  might  try  to  solve  this  problem  with  the  following  motion  strategy: 

•  Ram  the  gears  together.  See  if  they  mesh. 

Or,  somewhat  more  precisely, 

•  Ram  A  into  B.  If  they  mesh,  stop.  If  they  jam,  signal  failure  and  try  again. 
Probabilistically,  this  is  a  rather  good  strategy.  It  is  certainly  very  simple,  £md 

probably  easier  to  generate  than  a  guaranteed  strategy.  If  vision  can  be  used  to 
sense  whether  A  and  B  are  meshed,  then  it  is  an  EDR  strategy  with  just  one  step. 

Suppose,  however,  that  vision  is  poor,  or  that  the  gears  are  accessible  to  the 
robot  gripper,  but  not  to  the  camera.  This  means  that  position  sensing  will  be 
very  inaccurate,  and  hence  may  be  of  no  use  in  determining  whether  the  gears  are 
successfully  meshed.  This  will  often  be  the  case  in  practice.  In  this  case,  force 
sensing  must  be  used  to  disambiguate  the  success  of  the  motion  (meshing)  from 


failure  (jamming  in  an  immeshed  state).  If  the  robot  has  force  sensing,  then  it 
might  use  the  following  two-step  EDR  strategy: 

•  Ram  the  gears  together.  Spin  them  to  see  whether  they  meshed. 

Or,  again  more  precisely, 

•  Ram  A  into  B.  Next,  spin  A.  If  A  and  B  break  contact,  or  if  the  gears  stick 
( don’t  rotate ),  then  signal  failure.  Otherwise,  signal  success. 

This  strategy  is  essentially  the  one  that  LIMITED  generates.  The  plan  is 

Motion  1:  Command  a  pure  translation  of  A  into  B.^ 

Terminate  the  motion  based  on  force-sensing  when  sticking  occurs  (when  there 
is  no  motion). 

Motion  2:  Command  a  pure  rotation  of  A. 

If  breaking  contact  or  sticking  occurs,  signal  failure.  Otherwise,  signal  success. 

In  this  plan,  motion  (1)  does  not  terminate  distinguishably  in  success  (meshed) 
or  failure  (jammed).  That  is,  after  motion  (1)  terminates,  the  plan  executive  can¬ 
not  necessarily  recognize  whether  or  not  the  gears  are  meshed.  LIMITED  predicts 
this,  and  generates  motion  (2),  which  disambiguates  the  result  of  motion  (1).  The 
generation  of  the  second,  disambiguating  motion  involves  the  use  of  failure  mode 
analysis.  Breaking  contact  and  sticking  are  examples  of  failure  modes.  The  second 
motion  is  generated  so  that  from  any  unmeshed  state  resulting  from  motion  (1),  all 
possible  paths  will  terminate  distingiiishably  in  a  failure  mode.  Failure  mode  analy¬ 
sis  is  a  robust  subtheory  of  EDR  by  which  LIMITED  generates  multi-step  strategies 
under  pushing. 

1.1.3  Experiment:  Peg-in-HoIe  with  Model  Error 

This  section  describes  a  plan  that  was  generated  by  LIMITED  for  a  peg-in-hole 
problem  with  model  error.  It  gives  the  flavor  of  how  EDR  strategies  work.  Since 
pushing  motions  are  not  involved  here,  LIMITED  does  not  use  failure  mode  analysis 
to  solve  this  problem. 

Another  peg-in- hole  problem  is  depicted  in  fig.  3.  Again,  as  in  fig.  1,  there  is 
uncertainty  in  the  width  of  the  hole;  that  is,  the  width  is  known  to  lie  within  some 
given  interval.  In  addition,  there  are  chamfers  on  the  sides  of  the  hole.  The  depth 
of  the  chamfers  is  also  unknown,  but  we  are  given  bounds  on  the  depth.  Finally, 
the  exact  orientation  of  the  hole  is  uncertain.  The  geometry  of  the  holo  is  input 
to  the  planner  as  a  set  of  parametrically  defined  polygons.  They  are  defined  by  a 

*  LIMITED  generates  the  actual  force  vector. 


three  parameter  family,  for  width  of  the  hole,  depth  of  the  chamfers,  and  orientation 
of  the  hole.  An  associated  boimding  interval  is  also  input  for  each  parameter.  The 
geometry  of  the  peg  is  input  as  a  polygon. 

In  this  problem,  the  width  of  the  hole  may  be  smaller  than  the  width  of  the 
peg.  Thus  there  can  exist  no  strategy  that  is  guaranteed  to  succeed  for  all  geomet¬ 
ric  imcertainty  values.  However,  assvime  that  the  assembly — the  hole  geometry — 
is  inaccessible  to  robust  vision  or  position-measuring  devices.  In  particular,  the 
measurement  error  will  typically  determine  the  model  error  bounds,  which  in  this 
exzunple  are  large  for  the  purpose  of  illustration.  Thus  it  is  not  a  priori  possible 
to  measure  the  dimensions  ahead  of  time  to  determine  whether  or  not  the  assembly 
is  feasible.  Instead,  the  best  we  can  hope  for  is  an  EDR  strategy:  a  strategy  that 
takes  some  action  in  the  world  to  attempt  the  assembly,  but  whose  outcome  can  be 
recognizably  diagnosed  as  success  or  failure  by  the  run-time  robot  executor. 

The  peg  is  allowed  to  translate  in  the  plane.  Its  motion  is  modeled  using 
generalized  damper  dynamics.  This  permits  sliding  on  surfaces  about  the  hole. 
Friction  is  modeled  using  Coulomb’s  law.  With  these  dynamics  and  perfect  control, 
the  peg  would  exhibit  straight-line  motions  in  free  space,  followed  by  sliding  motions 
in  contact,  where  friction  permits.  Here,  however,  there  is  control  uncertainty,  which 
is  represented  by  a  cone  of  velocities.  Motions  in  free  speice  fan  out  in  a  kind  of 
“spray.”  Again,  sliding  is  possible  on  surfaces,  but  so  is  sticking,  depending  on 
the  effective  commanded  velocity  at  a  given  instant.  (In  this  case,  we  say  sliding 
is  non- deterministic).  The  size  of  the  control  uncertainty  cone  of  velocities  is  an 
input  to  the  planner.  Whether  sticking  may  occur  on  an  edge  may  be  computed  by 
intersecting  the  friction  cone  with  the  negative  control  imcertainty  cone. 

It  is  possible  to  sense  the  position  of  the  peg  and  the  forces  acting  on  it.  This 
information  is  only  approximate.  The  error  boimd  on  the  position  sensor  readings 
is  input  to  the  planner  sa  the  radius  of  a  disc. 

Limited  generates  plans  using  a  configuration  space  representation  of  the  con¬ 
straints  [Lozano-Perez].  In  the  plane,  one  imagines  shrinking  the  moving  object  to 
a  point,  and  correspondingly  “growing”  the  obstacles.  The  point  must  be  navigated 
through  free-space,  sliding  on  surfaces,  and  so  forth,  into  the  hole.  Fig.  4  shows 
configuration  spaces  for  different  parametric  variations  of  model  error.  Notice  that 
when  the  “real”  hole  is  too  small  for  the  peg  to  fit,  then  there  is  simply  no  hole  at 
all  in  the  corresponding  configuration  space.  Each  frame  in  fig.  4  is  called  a  “slice;” 
a  slice  represents  a  cross-section  where  the  model  error  parameters  are  constant.  To 
synthesize  an  EDR  strategy,  LIMITED  must  in  some  sense  consider  all  such  slices. 
In  practice  LIMITED  works  by  constructing  a  finite,  although  typically  large  number 
of  slices.  We  will  show  how  in  many  cases,  only  a  low  polynomial  number  need  be 
considered.  LIMITED  begins  by  considering  a  small  number  of  slices,  and  generates 
a  tentative  motion  strategy.  This  strategy  must  pass  a  test — which  we  call  the  EDR 


test — to  be  recognized  as  an  EDR  strategy.  One  of  the  chief  goals  of  this  thesis  is 
to  derive  this  test,  and  to  make  it  formal  and  2ilgorithmic.  Next,  LIMITED  attempts 
to  “generalize”  the  strategy  by  considering  successively  more  slices.  The  strategy  is 
modified  so  that  it  paisses  the  EDR  test  in  all  slices.  The  number  of  slices  considered 
is  the  resolution  of  the  planning.  This  approach  is  called  multi-resolution  planning. 

Let’s  consider  an  EDR  plan  that  LIMITED  computed  for  this  problem.  Figs. 
5-13  show  the  plan  graphically.  Qualitatively,  the  plan  may  be  described  as  follows; 

•  (1)  First,  move  left  and  slightly  down.  The  motion  will  terminate  on  the  left 
side  of  the  hole,  on  the  left  chamfers,  or  overshoot  the  hole  entirely.  Where  the 
motion  terminates  depends  both  on  the  trajectory  evolution  within  the  control 
uncertainty,  and  on  the  actual  geometry  of  the  hole.  The  motion  may,  how¬ 
ever,  slide  down  the  left  edge  of  the  hole  all  the  way  into  the  goal.  However, 
this  sliding  is  non- deterministic,  and  the  motion  may  stick  anywhere  along  that 
edge.  Since  the  first  motion  may  terminate  arbitrarily  close  to  the  goal  region, 
Limited  predicts  that  the  run-time  executive  system  cannot  necessarily  distin¬ 
guish  whether  or  not  the  first  motion  failed  to  achieve  the  goal. 

•  (2)  The  termination  regions  from  motion  (1)  are  taken  as  the  start  regions  for 
a  new  motion.  Next,  try  to  recover  by  commanding  a  motion  straight  down  and 
slightly  to  the  right.  This  motion  may  achieve  the  goal,  or  may  undershoot  it, 
or  may  overshoot  it.  The  second  motion  terminates  when  the  peg  sticks  on  a 
surface.  If  such  a  termination  surface  is  outside  the  goal,  it  is  called  a  failure 
region.  LIMITED  calculates  that  after  the  second  motion,  the  failure  regions 
are  distinguishable  from  the  goal  regions.  Hence  after  the  second  motion,  the 
run-time  executive  can  recognize  whether  or  not  the  plan  has  failed. 

Finally,  since  LIMITED  is  a  forward-chaining  planner,  it  is  possible  to  take  the 

failure  regions  from  motion  (2)  and  plan  a  third  recovery  motion.  Thus,  roughly 
speaking,  in  the  EDR  framework,  recovery  actions  are  planned  by  forward -chaining 
from  the  failure  regions  of  the  previous  motion.  When  the  failure  regions  are  poten¬ 
tially  indistinguishable  from  the  goal  (using  sensors),  then  the  recovery  action  must 
satisfy  the  formal  EDR  test  when  executed  from  the  union  of  the  goal  and  the  pre¬ 
vious  fmlure  regions.  For  example,  when  we  view  motion  strategies  as  “mappings” 
between  subsets  of  configuration  space,  then  typical  “robust”  recovery  actions  are 
EDR  plans  in  which  the  goal  is  a  “fixed  point. Motion  (2)  is  an  example  of  such 
a  one-step  EDR  plan. 

Figs.  5-13  show  the  plan  in  just  four  different  slices,  to  give  a  fiavor  for  the 
plan.  The  rest  of  the  slices  may  be  found  later  in  the  thesis.  Fig.  5  shows  the 
configuration  spaces  of  the  four  slices.  The  goal  region  here  is  shaded  black.  Note 
that  in  one  slice,  the  goal  disappears.  The  initial  uncertainty  in  the  position  of  the 

^That  is,  when  motion  (2)  originates  in  the  goal,  it  also  terminates  recognizably  in  the 
goal. 
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peg  is  represented  by  constraining  the  reference  point  (the  point  to  which  the  peg 
has  been  shrunk)  to  lie  in  one  of  the  start  regions  in  fig.  6. 

Figs.  7-8  represents  the  forward  projection  of  the  first  motion.  This  region  is 
the  outer  envelope  of  all  possible  trajectories  evolving  from  the  start  regions.  It 
is  the  set  of  all  configxirations  that  are  reachable  from  the  start  regions,  given  the 
commanded  velocity  and  control  uncertainty  cone. 

Fig.  9  shows  the  termination  regions  for  motion  (1).  The  termination  regions 
outside  the  goal  are  not  necessarily  distinguishable  from  the  goal. 

Figs.  10-11  show  the  forward  projection  of  the  second  motion. 

Fig.  12  shows  the  termination  regions  for  the  second  motion. 

Fig.  13  shows  the  size  of  the  position  sensing  uncertainty  ball.  The  goal  and 
the  failure  regions  in  fig.  12  are  distinguishable  using  sensors. 


Fig.  3.  A  peg-in-hole  environment  with  model  error.  The  width  of  the  hole 
(ai),  depth  of  chamfer  (aj),  and  orientation  of  the  hole  (03)  are  the  model 
parameters.  The  hole  is  allowed  to  close  up. 


Fig.  4a.  The  configuration  space  slices  for  many  different  parametric  model  error 
values.  These  configuration  spaces  were  generated  for  the  peg-in-hole  problem 
with  model  error  depicted  in  fig.  3.  Fig.  4a  shows  a  few  slices  taken  at  constant 
orientation,  whereas  in  fig.  4b,  more  slices  are  shown  at  various  orientations. 
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Fig.  7.  The  forward  projection  of  the  first  motion.  This  region  is  the  outer 
envelope  of  all  pouible  trajectories  evolving  from  the  start  regions.  It  is  the 
set  of  all  configurations  that  are  reachable  from  the  start  regions,  given  the 
commanded  velocity  and  control  uncertainty  cone. 


1.2.  Research  Issues 


The  gross  motion  planning  problem  with  no  imcertainty  has  received  a  great 
deal  of  attention  recently.  In  this  problem,  the  state  of  the  robot  may  be  represented 
as  a  point  in  a  configimation  space.  Thus  moving  from  a  start  to  a  goal  point  may  be 
viewed  {is  finding  an  {uc  in  free  space  connecting  the  two  points.  Since  the  robot  is 
assumed  to  have  perfect  control  and  sensing,  any  such  arc  may  be  rehably  executed 
once  it  is  foimd.  In  particular,  given  a  candidate  arc,  it  may  be  tested.  That 
is,  motion  {dong  the  arc  may  be  simulated  to  see  whether  it  is  collision  free.  For 
ex{unple,  {m  algebraic  curve  may  be  intersected  with  semi-{dgebraic  sets  defining  the 
configuration  space  obstacles.  In  the  presence  of  uncertainty,  however,  we  cannot 
simply  simulate  a  motion  strategy  to  verify  it.  Inste{id,  we  need  some  technique  for 
simulating  all  possible  orbits,  or  evolutions  of  the  robot  system,  under  amy  possible 
choice  of  the  vmcertain  parameters.  With  sensing  and  control  uncertainty,  the  state 
of  the  robot  must  be  viewed  as  a  subset  of  the  configuration  sp{u:e.  Motions,  then, 
can  be  viewed  as  mappings  between  these  subsets.  Of  coinse  there  au-e  many  such 
subsets!  From  this  perspective,  it  is  clear  that  a  chief  contribution  of  [LMT]  has  been 
to  identify  and  give  a  constructive  definition  for  a  privileged  class  of  subsets,  called 
preimagea,  and  show  that  it  is  necessary  and  sufficient  to  seairch  among  this  claiss. 
This  framework  appears  very  promising  for  planning  guaranteed  motion  strategies 
under  sensing  aind  control  uncertainty.  The  [LMT]  framework  assumes  no  model 
error.  In  this  thesis,  we  reduce  the  problem  of  planning  guaranteed  strategies  with 
sensing,  control,  and  geometric  model  uncertainty  to  the  problem  of  computing 
preimages  in  a  (higher  dimensional)  generalized  configuration  space.  ® 

This  is  an  interesting  and  useful  result;  previously,  there  w{is  really  no  system¬ 
atic  theory  of  planning  in  an  environment  whose  geometry  is  not  precisely  known. 
However,  I  do  not  think  that  it  is  the  main  point  of  this  thesis.  This  is  because  there 
are  certain  inadequacies  with  the  pl{mning  model.  The  insistence  that  strategies 
be  guaranteed  to  succeed  is  too  restrictive  in  practice.  To  see  this,  observe  that 
guaranteed  strategies  do  not  always  exist.  In  the  peg-in-hole  problem  with  model 
error  (figs.  3-13)  there  is  no  guaranteed  strategy  for  achieving  the  goal,  since  the 
hole  may  be  too  small  for  some  model  error  values.  For  these  values  the  goal  in 
configuration  space  does  not  exist.  Because  tolerances  may  cause  gross  topological 
changes  in  configuration  space,  this  problem  is  particularly  prevalent  in  the  pres¬ 
ence  of  model  error.  More  generally,  there  may  be  model  error  values  for  which  the 
goal  may  still  exist,  but  it  may  not  be  reachable.  For  example,  in  a  variant  of  the 
problem  in  fig.  3,  an  obstacle  could  block  the  channel  to  the  goal.  Then  the  goal  is 
non-empty,  but  also  not  reachable.  Finally,  and  most  generally,  there  may  be  model 
error  values  for  which  the  goal  is  reachable  but  not  recognizably  reachable.  In  this 
c{ise  we  still  carmot  guarantee  plans,  since  a  plarmer  cannot  know  when  they  have 


succeeded. 

These  problems  may  occur  even  in  the  absence  of  model  error.  However,  with¬ 
out  model  error  a  guaranteed  plan  is  often  obtainable  by  back-chaining  and  adding 
more  steps  to  the  plan.  In  the  presence  of  model  error  this  technique  frequently 
fails:  in  the  peg-in-hole  problem  witi*  xlel  error,  this  technique  will  not  work  since 
no  plan  of  any  length  can  succeed  when  the  hole  closes  up. 

This  is  why  we  investigate  EDR  strategies,  and,  in  particular,  attempt  to  for¬ 
malize  EDR  planning.  The  key  theoretical  issue  is:  How  can  we  relax  the  restriction 
that  plans  must  be  guaranteed  to  succeed,  and  still  retain  a  theory  of  planning  that 
is  not  completely  ad  hoc?  We  attempt  to  answer  this  by  giving  a  constructive  def¬ 
inition  of  EDR  strategies.  In  particular,  this  approach  provides  a  formal  test  for 
verifying  whether  a  given  strategy  is  an  EDR  strategy.  The  test  is  formulated  as  a 
decision  problem  about  projection  sets  in  a  generalized  configuration  space  which 
also  encodes  model  error.  Roughly  speaking,  the  projection  sets  represent  all  p>os- 
sible  outcomes  of  a  motion  (the  forward  projection),  and  weakest  preconditions  for 
attaining  a  subgoal  (the  preimage). 

Given  the  formal  test  for  “recognizing”  an  EDR  strategy,  I  then  tested  the 
definition  by  building  a  generate-and-test  planner.  The  generator  is  trivial;  the 
recognizer  is  an  algorithmic  embodiment  of  the  formal  test.  It  lies  at  the  heart 
of  this  research.  A  second  key  component  of  the  planner  is  a  set  of  techniques 
for  chaining  together  motions  to  synthesize  multi-step  strategies.  The  planner  is  a 
forward-chaining,  multi-resolution  planner,  called  LIMITED.  LIMITED  operates  in  a 
restricted  domain.  Plans  found  by  LIMITED  in  experiments  are  described  above, 
and  in  chapters  II  and  III. 

Finally,  let  me  suggest  that  a  new  framework — the  EDR  framework —  for 
planning  with  uncertainty  may  be  justified  not  only  by  the  restrictiveness  of  the 
guaranteed-success  model,  but  also  by  the  hardness  of  the  problem.  The  gross  mo¬ 
tion  planning  problem  without  uncertainty  may  be  viewed,  under  some  very  generaJ 
assumptions,  as  a  decision  problem  within  the  theory  of  real  closed  fields.  This  gives 
a  theoretical  decision  procedure  with  polynomial  nmning  time  once  the  degrees  of 
freedom  of  the  robot  system  are  fixed  (SSj.  However,  no  such  theoretical  algorithm 
is  known  for  the  general  compliant  motion  planning  problem  with  uncertainty.  Fur¬ 
thermore,  the  lower  bounds  for  computing  giiaranteed  strategies  even  in  3D  are 
dismal:  the  problem  is  known  to  be  hard  for  exponential  time  [CR].  At  this  point 
it  is  unknown  whether  EDR  planning  is  more  efficient  than  guaranteed  planning. 
However,  there  is  some  experimental  evidence  leading  one  to  conjecture  that  cer¬ 
tain  problems  requiring  very  complicated,  exponential-sized  guaranteed  plans  may 
admit  very  short  EDR  plans. 

However,  the  motivation  for  this  thesis  is  not  complexity-theoretic.  Instead, 
the  chief  thrust  is  to  show  how  to  compute  motion  strategies  under  model  error 


(and  sensing  and  control  uncertainty),  using  a  formal  and  constructive  definition 
of  EDR  strategies.  The  first  goal  was  a  precise  geometric  characterization  of  EDR 
planning — when  one  thinks  about  it,  it  is  in  fact  somewhat  surprising  that  such 
a  thing  should  exist  at  all!  But  in  fact  it  does,  as  we  shall  see.  The  second  goal 
was  to  test  this  characterization  by  building  a  planner.  Thus  it  was  necessary  to 
devise  implementable  algorithms  to  construct  the  geometric  projection  sets  and 
decide  questions  about  them.  Therefore,  this  thesis  and  LIMITED  contain  a  mixture 
of  precise  combinatorial  algorithms  and  of  approximation  algorithms.  We  indicate 
which  algorithms  axe  exact  and  give  combinatorial  bounds.  We  also  identify  the 
approximation  algorithms,  and  indicate  the  goodness  of  the  approximation  and 
whether  it  is  conservative.  Much  work,  or  course,  remains  in  developing  better 
algorithms  for  EDR  planning,  and  in  testing  out  the  plans  using  real  robots. 

1.3.  Review  of  Previous  Work 

Broadly  speaking,  previoiis  work  falls  the  following  categories:  Algorithmic 
motion  planning,  Compliant  motion  planning  with  uncertainty.  Model  Error,  and 
Error  detection  and  recovery. 

1.3.1  Algorithmic  Motion  Planning 

In  algorithmic  motion  planning,  (also  called  the  piano  movers’  problem,  or  the 
find-path  problem)  the  problem  is  to  find  a  continuous,  collision-free  path  for  a 
moving  object  (the  robot)  amidst  completely  known  polyhedral  or  semi-algebraic 
obstzw:les.  It  is  assumed  than  once  such  a  path  is  found,  it  can  be  rehably  executed 
by  a  robot  with  perfect  control  and  sensing.  Many  algorithms  employ  configxuration 
space,  [Lozano-Perez,  Arnold,  Abraham  and  Marsden,  Udupa].  [Lozano-Perez  and 
Wesley]  proposed  the  first  algorithms  for  polygoneil  and  polyhedral  robots  and  ob¬ 
stacles  without  rotations.  These  results  were  later  extended  by  [Loz^mo-Perez  81, 
83]  to  polyhedral  robots  which  could  translate  and  rotate.  [Brooks  83]  designed  a 
find-path  algorithm  based  on  a  generalized-cone  representation  of  free-space.  Brooks 
later  extended  this  method  for  a  revolute-joint  robot.  [Donald  84,85,87]  developed 
a  mot  ion- planning  algorithm  for  a  rigid  body  that  could  translate  and  rotate  with 
six  degrees  of  freedom  amidst  polyhedral  obstacles  (the  so-called  “classical”  movers’ 
problem).  [Lozano-Perez  85]  reported  another  6D0F  algorithm  for  6-link  revolute 
manipulators.  [Canny  85]  developed  an  algebraic  formulation  of  the  configuration- 
space  constraints,  which  led  to  a  very  clean  collision-detection  algorithm.  All  of 
these  algorithms  have  been  implemented. 

There  are  many  theoretical  results  on  upper  and  lower  bounds  for  the  find-path 
problem,  see  [Yap]  for  a  good  survey  article.  These  results  begin  with  [Lozano-Perez 


zind  Wesley],  who  give  the  first  upper  bounds:  they  give  efficient  algorithms  for  plan¬ 
ning  in  2D  and  3D  in  the  absence  of  rotations.  [Reif  79]  obtained  the  first  lower 
bounds,  demonstrating  the  problem  to  be  'PSVACE-h.sxd  when  the  number  of  de¬ 
grees  of  freedom  are  encoded  in  the  input  specification  of  the  problem.  [Hopcroft, 
Joseph,  and  Whitesides]  and  [Hopcroft,  Schwartz,  and  Sharir]  have  also  given  in- 
tersesting  lower  bounds  for  motion  planning.  [Schwartz  and  Sharir]  gave  a  very 
general  theoreticaJ  algorithm  for  motion  planning  via  a  reduction  to  the  theory  of 
real  closed  fields.  The  algorithm  is  doubly-exponential  in  the  degrees  of  freedom, 
but  polynomial  in  the  algebraic  and  geometric  complexity  of  the  input.  Over  the 
next  five  years,  there  were  many  papers  reporting  more  efficient  special-purpose 
motion  planning  algorithms  for  certain  specific  cases;  see  [Yap]  for  a  survey.  To 
date  the  fastest  general  algorithm  is  due  to  [Canny,  87],  who  gives  a  generic  motion 
planning  algorithm  which  is  merely  singly-exponential  in  the  degrees  of  freedom. 
For  a  motion  planning  problem  of  algebraic  complexity  d,  geometric  complexity 
n,  and  with  r  degrees  of  freedom.  Canny’s  zilgorithm  runs  in  time  ^n’'logn) 

which  is  within  a  log  factor  of  optimal.  While  none  of  these  theoretical  algorithms 
have  been  implemented,  Caimy’s  is  conjectured  to  be  efficient  in  practice  as  well. 

One  might  ask  whether  exact  algorithms  for  motion  plannning  can  ever  be 
utilized  after  imcertainty  in  sensing  and  control  are  introduced.  The  answer  is  a 
qualified  “yes.”  In  particular,  the  Voronoi  diagram  has  proved  to  be  useful  for  mo¬ 
tion  planning  among  a  set  of  obstacles  in  configuration  space  (see  [O’Dunlaing  and 
Yap  82;  O’Dunlaing,  Sharir,  and  Yap  84;  Yap  84],  and  the  textbook  of  [Schwartz 
and  Yap  86]  for  an  introduction  and  review  of  the  use  of  Voronoi  diagrams  in  mo¬ 
tion  planning).  The  Voronoi  diagram,  as  usually  defined,  is  a  strong  deformation 
retract  of  free  space  so  that  free  space  can  be  continuously  deformed  onto  the  dia¬ 
gram.  This  means  that  the  diagram  is  complete  for  path  planning,  i.e.  Searching 
the  original  space  for  paths  can  be  reduced  to  a  search  on  the  diagram.  Reducing 
the  dimension  of  the  set  to  be  searched  usually  reduces  the  time  complexity  of  the 
search.  Secondly,  the  diagram  leads  to  robust  paths,  i.e.  paths  that  are  maximally 
clear  of  obstacles.  Hence  Voronoi- beised  motion  planning  algorithms  are  relevant  to 
motion  planning  with  uncertainty.  [Canny  and  Donald]  define  a  “Simplified  Voronoi 
Diagram”  which  is  still  complete  for  motion  planning,  yet  heis  lower  algebraic  com¬ 
plexity  than  the  usual  Voronoi  diagram,  which  is  a  considerable  advantage  in  motion 
planning  problems  with  many  degrees  of  freedom.  Furthermore,  the  Simplified  di¬ 
agram  is  defined  for  the  6D  configuration  space  of  the  “classical”  movers’  problem. 
For  the  6D0F  “classical”  polyhedral  case,  [Canny  2ind  Donald]  show  that  motion 
planning  using  the  Simplified  diagram  can  be  done  it  time  0(n^  logn). 

Many  additional  robotics  issues  are  discussed  in  [Paul;  Brady  et  a/.]. 


1.3.2  Compliant  Motion  Planning  with  Uncertainty 

This  section  reviews  previous  work  on  planning  compliant  motions  which  are 
guaranteed  to  succeed  even  when  the  robot  system  is  subject  to  sensing  and  control 
uncertainty.  All  of  this  work  assumes  perfect  geometric  models  of  the  robot  and 
obstacles. 

Work  on  compliant  motion  can  be  traced  to  [Inoue,  Whitney,  Raibert  and 
Craig,  Salisbury].  This  work  in  force  control  attempted  to  use  the  geometric  con¬ 
straints  to  guide  the  motion.  By  cleverly  exploiting  the  task  geometry,  placements 
far  exceeding  the  accuracy  of  pure  position  control  can  be  achieved.  [Mason  83] 
develops  spring  and  damper  compliance  models,  and  gives  an  extensive  review  of  re¬ 
search  in  compliant  motion.  [Simunovic,  Whitney,  Ohwovoriole  and  Roth,  Ohwovo- 
riole.  Hill  and  Roth]  have  all  considered  frictional  constraints,  as  well  as  jamming 
and  wedging  conditions.  [Erdmann],  [Burridge,  Rajan  and  Schwartz]  have  consid¬ 
ered  algorithmic  techniques  for  predicting  reaction  forces  in  the  presence  of  friction. 
[Caine]  has  considered  manual  techniques  for  synthesizing  compliant  motion  strate¬ 
gies,  generalizing  the  methods  of  [Simunovic,  Whitney].  [Mason,  82]  has  developed 
a  way  to  model  pushing  and  grasping  operations  in  the  presence  of  frictional  con¬ 
tact.  [Peshkin]  h2is  extended  this  work.  [Brost]  has  further  developed  techniques 
for  predicting  pushing  and  sliding  of  manipulated  objects  to  plan  squeeze-grasp 
operations.  In  addition,  Brost  is  currently  investigating  the  application  of  EDR 
techniques  to  the  squeeze-grasp  domain. 

Early  work  on  planning  in  the  presence  of  imcertainty  investigated  using  skele¬ 
ton  strategies.  [Lozano-Perez  76]  proposed  a  task-level  planner  called  LAMA  which 
used  geometric  simulation  to  predict  the  outcomes  of  plans,  and  is  one  of  the  earliest 
systems  to  address  EDR  planning.  [Taylor]  used  symbolic  reasoning  to  restrict  the 
values  of  variables  in  skeleton  plans  to  guarantee  success.  [Brooks  82]  later  extended 
this  technique  using  a  symbolic  algebra  system.  [Dufay  and  Latombe]  implemented 
a  system  which  addresses  learning  in  the  domain  of  robot  motion  planning  with 
uncertainty. 

[LMT]  proposed  a  formal  framework  for  automatically  synthesizing  fine-motion 
strategies  in  the  presence  of  sensing  and  control  imcertainty.  Their  method  is  called 
the  preimage  framework.  [Mason,  83]  further  developed  the  preimage  termination 
predicates,  addressing  completeness  and  correctness  of  the  resulting  plans.  [Erd¬ 
mann]  continued  work  on  the  preimage  framework,  and  demonstrated  how  to  sep¬ 
arate  the  problem  into  questions  of  reachability  and  recognizability.  He  also  showed 
how  to  compute  preimages  using  backprojections,  which  address  reachability  alone, 
and  designed  and  implemented  the  first  algorithms  for  computing  backprojections. 
[Erdmann  and  Mason]  developed  a  planner  which  could  perform  sensorless  ma¬ 
nipulation  of  polygonal  objects  in  a  tray.  Their  planner  makes  extensive  use  of  a 
representation  of  friction  in  configuration  space  [Erdmann].  [Buckley]  implemented 


a  multi-step  planner  for  planning  compliant  motions  with  uncertainty  in  CD  with¬ 
out  rotations.  He  also  developed  a  variety  of  new  theoretical  tools,  including  a 
combined  spring-damper  dynamic  model,  3D  backprojection  and  forward  projec¬ 
tion  8dgorithms,  and  a  finitization  technique  which  makes  searching  the  space  of 
commanded  motions  more  tractable. 

[Hopcroft  and  Wilfong]  addressed  the  problem  of  plarming  motions  in  contact, 
and  proved  important  structural  theorems  about  the  connectivity  of  the  1-edges  of 
configuration  space  obstacle  manifolds.  [Koutsou]  has  suggested  a  planning  algo¬ 
rithm  which  plans  along  1-edges.  Other  planning  systems  for  compliant  motion  have 
been  developed  by  [Turk],  who  used  backprojections,  [Laugier  and  Theveneau],  who 
use  2in  expert  system  for  geometric  reasoning  about  compliant  motion,  and  [Valadej. 

Recently,  there  has  been  some  theoretical  work  on  the  complexity  of  robot 
motion  planning  with  imcertainty.  [Erdmann]  showed  the  problem  to  be  imdecidable 
when  the  obstacles  are  encoded  as  a  recursive  function  on  the  plane.  [Natarajan] 
has  shown  the  problem  to  be  VSVACE-h&td  in  3D  for  finite  polyhedral  obstacles. 
[Canny  and  Reif]  have  demonstrated  that  in  3D  the  problem  of  synthesizing  a  multi- 
step  strategy  is  hard  for  non-deterministic  exponential  time;  in  addition,  they  proved 
that  verifying  a  1-step  strategy  is  MV-hoxA. 

1.3.3  Model  Error 

There  is  relatively  little  previous  work  on  planning  in  the  presence  of  model 
uncertainty.  [Requicha]  and  [Shapiro]  address  representational  questions  of  how  to 
model  part  tolerances,  and  mathematical  models  for  variational  families  of  parts. 
[Buckley]  considers  some  extensions  of  his  planner  to  domains  with  model  uncer¬ 
tainty.  [Brooks  82]  developed  a  symbolic  algebra  system  which  can  constrain  the 
variable  values  in  skeleton  plans,  and  introduce  sensing  and  motion  steps  to  reduce 
these  values  imtil  the  error  ranges  are  small  enough  for  the  plan  to  be  guaranteed. 
Some  of  the  variables  in  these  plans  can  represent  model  error — particularly,  the  po¬ 
sition  of  objects  in  the  workspace — and  hence  his  planner  can  reason  about  motion 
planning  in  the  presence  of  model  uncertainty. 

Work  on  manipulator  pushing  and  sliding  [Mason,  Peshkin]  and  squeeze-grasping 
[Brost]  may  be  viewed  as  addressing  model  error  where  the  error  parameters  are  the 
position  and  orientation  of  the  manipulated  part.  The  operation  space  of  [Brost] 
is  a  clever  example  how  to  model  actions  with  uncertain  effects,  and  objects  with 
uncertain  orientation,  in  the  same  space.  [Durrant-Whyte]  considers  how  to  model 
geometric  uncertainty  probabilistically,  and  how  to  propagate  such  information  in 
applications  related  to  motion  planning. 

[Lumelsky]  considers  the  following  problem:  suppose  that  a  robot  has  a  2D 
configuration  space,  perfect  control  and  sensing,  the  obstacles  are  fiiiite  in  number. 


and  each  obstacle  boundary  is  a  homeomorphic  image  of  the  circle.  Then  a  collision 
free-path  may  be  found  by  tracing  around  the  boundary  of  any  obstacles  encoun¬ 
tered  when  moving  in  a  straight  line  from  the  start  to  the  goal.  At  eax:h  obstacle 
boundary  encountered,  there  is  a  binary  choice  of  which  way  to  go,  and  the  move 
may  be  executed  with  perfect  accuracy.  Lumelsky  also  demonstrates  complexity 
boimds  under  these  assumptions,  and  hcis  considered  configuration  spaces  such  as 
the  plane,  the  sphere,  the  cylinder,  and  the  2-torus.  While  it  is  not  cleeir  how 
this  technique  can  extend  to  higher-dimensional  configuration  spaces,  it  is  useful  to 
compare  Lumelsky’s  approach  as  an  example  of  how  to  exploit  a  useful  geometric 
primitive  (wall-following).  See  also  [Koditschek]  for  extensions  to  this  approach 
using  potential  fields.  The  potential-field  approach  to  collision  avoidance,  as  formu¬ 
lated  by  [Khatib],  also  can  deal  with  uncertain  obstacles,  and  gross  motions  aroimd 
these  obstacles  cem  often  be  synthesized  in  read  time.  [Brooks  85]  has  described  a 
map-making  approach  for  a  mobile  robot  in  a  highly  unstructured  environment — 
i.e.,  amidst  unknown  obstacles.  His  approach  allows  the  robot  to  aquire  information 
about  the  position  and  shape  of  these  obstacles  as  the  robot  explores  the  environ¬ 
ment.  [Davis]  has  addressed  the  mobile  robot  navigation  problem  amidst  partially 
unknown  obstacles  using  an  approximate  map. 

There  is  almost  no  work  on  planning  compliant  motions  or  assemblies  in  the 
presence  of  model  error. 

1.3.4  Error  Detection  and  Recovery 

There  has  been  almost  no  formal  an2ilysis  of  the  EDR  problem.  STRIPS  [Fikes 
and  Nilsson]  has  a  run-time  executive  (PLANEX)  which  embodied  one  of  the  first 
systems  addressing  EDR.  STRIPS’  triangle  tables  may  be  viewed  as  a  kind  of  for¬ 
ward  projection.  [Ward  and  McCalla;  Hayes]  have  presented  research  agendas  for 
error  diagnosis  and  recovery  in  domain-independent  planning.  [McDermott]  has 
stressed  the  importance  of  EDR  in  plan  execution  and  sketched  an  approach  based 
on  possible  worlds.  [Srinivas]  described  a  robot  planning  system  for  a  Mars  rover 
which  could  detect  certain  manipulation  errors  and  recover.  [Gini  and  Gini]  have 
described  a  view  of  EDR  based  on  a  predetermined  list  of  high-level  error  types. 
The  domain-independent  planning  literature  [Chapman]  is  relevant  to  the  history  of 
EDR;  for  example,  the  planner  of  [Wilkins]  has  an  error  recovery  module  in  which 
the  executor  c2Ln  detect  inconsisteitcies  in  the  set  of  logical  propositions  representing 
the  world  state.  At  this  point,  an  operator  can  intervene  and  type  in  new  proposi¬ 
tions  to  disambiguate  the  state  and  aid  recovery.  The  robots  described  by  [Brooks 
85]  have  an  EDR  flavor — they  are  not  required  to  achieve  a  particular  goal,  but 
merely  to  attempt  it  until  some  other  goal  takes  a  higher  priority. 

Portions  of  the  material  in  this  thesis  have  been  presented  in  [Donald  86a, b]. 
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[Brost]  is  employing  these  EDR  techniques  in  his  research  on  planning  squeeze-grasp 
operations. 

1.4.  Map  of  Thesis 

Here  is  an  outline  of  the  remainder  of  the  thesis.  The  thesis  is  divided  into 
six  roman-numeral  chaptcTs.  A  parallel  arabic  jection-numbering  scheme  permits 
finer-grain  cross-references.  Starred  (♦)  sections  subsections  may  be  skipped  at  first 
reading  if  desired. 

Chapter  II  presents  the  basic  issues  in  EDR.  It  begins  with  a  discussion  of 
pleinning  with  model  error.  We  introduce  a  generalized  configuration  space  with 
non-holonomic  constraints  as  a  key  tool.  EDR  is  defined,  and  given  a  geometrical 
characterization.  Experiments,  implementation,  and  computational  complexity  eure 
discussed.  Chapter  II  intends  to  provide  a  slice  of  all  the  most  interesting  aspects 
of  this  work,  while  striving  for  a  somewhat  informal  style  of  presentation.  The  end 
of  the  chapter  hints  at  the  theoretical  issues  to  come. 

The  chapters  III — V,  can  be  read  independently  if  desired. 

Chapter  III  describes  the  construction  of  multi-step  strategies  in  some  detail. 
Here,  we  discuss  plaxming  using  preimages,  “push-forward”  algorithms,  and  failure¬ 
mode  analysis.  These  techniques  are  then  unified  by  introducing  the  “weak”  EDR 
theory.  Weak  EDR  is  a  theory  which  defines  certain  laws  of  composition  on  motion 
strategies.  It  provides  a  new  framework  for  studying  multi-step  strategies;  we  use 
it  to  derive  properties  of  multi-step  EDR  plans. 

In  chapter  IV,  the  EDR  theory  is  applied  to  the  problem  of  planning  sensing 
and  motion  for  a  mobile  robot  navigating  amidst  partially  unknown  obstacles.  We 
show  how  the  EDR  theory,  and  generalized  configuration  space  in  particular,  can 
be  used  to  generate  strategies  in  the  mobile  robot  dommn. 

Chapter  V  describes  implementational  and  complexity-theoretic  issues.  We 
discuss  methods  for  limiting  search  in  an  EDR  planner.  To  this  end,  we  introduce 
a  combinatorial  object  Ccill  the  non- directional  backprojection,  and  analyze  its  com¬ 
plexity.  Our  analysis  leads  to  efficient  algorithms  for  certain  subproblems  in  EDR 
planning.  In  particular,  we  give  an  efficient  algorithm  for  planning  one-step  (guar¬ 
anteed)  strategies  in  the  plane.  By  using  results  from  computational  algebra,  we 
show  that  planning  a  guaranteed  planar  multi-step  strategy  with  sticking  termina¬ 
tion  can  be  decided  in  time  polynomial  in  the  geometric  complexity,  and  roughly 
singly-exponentiai  in  the  number  of  steps  in  the  plan. 

Chapter  VI  contains  conclusions  and  suggestions  for  future  work. 

All  readers  should  be  able  to  read  through  chapter  II.  At  that  point  the  re¬ 
maining  topics  can  be  selected  as  the  reader's  taste  and  preference  dictate.  I  feel  the 
most  interesting  and  important  subsequent  material  is  on  the  weak  EDR  theory'  (in 


the  multi-step  strategy  chapter  III).  However,  readers  interested  in  computational 
complexity  might  prefer  chapter  V,  while  moboticists  might  skip  to  chapter  IV. 

The  thesis  contains  three  thematic  lines  of  development.  The  first  is  theoretical 
robotics,  by  which  we  mean  the  theory  of  manipulation  and  geometrical  planning. 
This  hne  is  strong  in  chapters  II  and  III.  Readers  who  have  seen  some  of  this  thesis 
material  at  conferences  [D]  will  find  altogether  new  material  in  sections  7-15.  This 
line  of  development  contains  the  following  topics: 

•  Model  error  is  discussed  in  detail  in  sections  2,  6,  9,  11,  and  13. 

•  The  basic  EDR  theory  is  discussed  in  sections  3-5,  and  7-10. 

•  Failure-Mode  Analysis  comes  up  in  section  11. 

•  The  Weak  EDR  Theory  makes  its  debut  in  section  12. 

The  second  theme  is  complexity  and  algorithmic  issues.  These  are  stressed  in 
sections  6  and  14. 

The  third  theme  is  applications,  implementation,  and  experiments.  These  are 
described  in  sections  6,  7,  10,  11,  13,  and  14.1. 


II.  Basic  Issues  in  Error  Detection  and  Recovery 


This  chapter  presents  an  overview  of  our  theory  and  experiments  in  EDR.  It 
attempts  to  deliver  a  slice  of  all  the  most  interesting  aspects  of  the  work,  while 
striving  for  a  non-demanding  style  of  presentation. 

We  begin  by  showing  how  to  represent  model  error,  and  explore  the  physics 
of  generalized  configuration  space.  Using  this  representation,  we  next  present  the 
b^lsic  theory  behind  constructing  both  guaranteed  strategies  emd  EDR  plans  in  the 
presence  of  sensing,  control,  and  model  uncertainty. 

The  implementation  of  LIMITED  is  then  discussed,  Eilong  with  experiments  in 
EDR  planning. 

The  chapter  closes  by  proving  complexity  bounds  for  EDR  planning,  and  with 
an  introductory  discussion  of  deeper  EDR-theoretic  issues.  For  interested  readers, 
these  discussions  cire  continued  and  elaborated  in  considerable  detail  in  chapters  V 
and  III,  respectively. 


2.  Baisic  Issues  in  Error  Detection  and  Recovery 


2.1.  Simple  Example  of  Model  Error 

We  will  begin  developing  the  EDR  theory  by  examining  some  very  simple 
planning  problems  with  model  error.  Of  course,  this  does  not  mean  that  EDR  is 
hmited  to  situations  with  model  error. 

Example  (1).  Consider  fig.  14.  There  is  position  sensing  imcertmnty,  so  that 
the  start  position  of  the  robot  is  only  known  to  lie  within  some  ball  in  the  plane. 
The  goal  is  to  bring  the  robot  in  contact  with  the  right  vertical  surface  of  A. 

We  will  simpUfy  the  problem  so  that  the  computational  task  is  in  configuration 
space.  This  transformation  reduces  the  planning  task  for  a  complicated  moving 
object  to  navigating  a  point  in  configuration  space.  Consider  fig.  15.  The  config- 
vuation  point  starts  out  in  the  region  R,  which  is  the  position  sensing  imcertainty 
ball  about  some  initial  sensed  position.  To  model  sliding  behavior,  we  will  assume 
Coulomb  friction  and  generalized  damper  dynamics,  which  allows  an  identification 
of  forces  and  velocities.  Thus  the  commanded  velocity  vo  is  related  to  the  effective 
velocity  v  hy  f  =  B(v  —  vq)  where  /  is  the  effective  force  on  the  robot  and  B  is  a 
scalar.  Given  a  nominal  commanded  velocity  Vq,  the  control  imcertainty  is  repre¬ 
sented  by  a  cone  of  velocities  {B^c  in  the  figure).  The  actual  commanded  velocity 
Vo  must  lie  within  this  cone.^ 

The  go2il  in  fig.  15  is  to  move  to  the  region  G.  Now,  with  Coulomb  friction, 
sticking  occurs  on  a  surface  when  the  (actual)  commanded  velocity  points  into  the 
friction  cone.  We  assume  the  friction  cones  are  such  that  sliding  occurs  (for  all 
possible  commanded  velocities  in  B^c)  on  all  surf^lces  save  G,  where  all  velocities 
stick.  We  will  assume  that  the  planner  can  monitor  position  and  velocity  sensors  to 
determine  whether  a  motion  has  reached  the  goal.  Velocity  sensing  is  also  subject 
to  imcertainty:  for  an  2u:tual  velocity  v,  the  sensed  velocity  lies  in  some  cone  Btv 
of  velocities  about  v. 

Now  we  introduce  simple  model  error.  The  shape  of  A  and  B  are  known 
precisely,  and  the  position  of  A  is  fixed.  However,  the  position  of  B,  relative  to  A 
is  not  known.  B’s  position  is  characterized  by  the  distance  a.  If  q  >  0  the  goal 
is  reachable.  But  if  a  =  0,  then  the  goal  vanishes.  No  plan  can  be  guaranteed  to 
succeed  if  a  =  0  is  possible.  Suppose  we  allow  a  to  be  negative.  In  this  case  the 
blocks  meet  and  fuse.  Eventually,  for  sufficiently  negative  a,  B  will  emerge  on  the 
other  side  of  A.  In  this  case,  the  goal  “reappears,”  and  may  be  reachable  again.^ 
Let  us  assume  that  a  is  bounded,  and  lies  in  the  interval  [—do?  do]. 

*See  [Mason  81]  for  a  detailed  description  of  generalized  damper  dynamics. 

^This  model  is  adopted  for  the  purposes  of  exposition,  not  for  physical  plausibility.  It  is 
not  hard  to  model  the  case  where  the  blocks  meet  but  do  not  fuse. 


Our  task  is  to  find  a  plan  that  can  attain  G  in  the  czises  where  it  is  recognizably 
reachable.  Such  a  plan  is  called  a  guaranteed  strategy  in  the  presence  of  model  error. 
But  the  plan  cannot  be  guaranteed  for  the  a  where  the  goal  vanishes.  In  these 
cases  we  w£int  the  plam  to  signal  failure.  Loosely  speaking,  a  motion  strategy  which 
achieves  the  goal  when  it  is  recognizably  reachable  eind  signals  failure  when  it  is 
not  is  called  an  Error  Detection  and  Recovery  (EDR)  strategy.  Such  strategies  are 
more  general  than  guaranteed  strategies,  in  that  they  allow  plans  to  fail. 

Before  we  attack  the  problem  of  constructing  guaranteed  strategies  and  EDR 
strategies  (both  in  the  presence  of  model  error)  let  us  consider  the  examples  we 
have  seen  so  far.  Although  in  these  examples  model  error  has  been  represented  by 
a  kind  of  parametric  “tolerancing” ,  the  planning  framework  can  represent  arbitrary 
model  error.  For  example,  we  could  represent  CAD  surfaces  with  real  coefficients, 
and  allow  the  coefficients  to  vary.  Discrete  and  discontinuous  model  error  may 
also  be  represented.  Finally,  note  that  we  permit  gross  topological  changes  in  the 
environment — for  example,  the  goal  can  vanish. 

2.2.  Representing  Model  Error 

To  represent  model  error,  we  will  choose  a  parameterization  of  the  possible 
variation  in  the  environment.  The  degrees  of  freedom  of  this  parameterization  are 
considered  as  additional  degrees  of  freedom  in  the  system.  For  example,  in  fig.  15, 
we  have  the  x  and  y  degrees  of  freedom  of  the  configuration  space.  In  addition,  we 
have  the  model  error  parameter  a.  A  coordinate  in  this  space  has  the  form  (ir,  y,  a). 
The  space  itself  is  the  cartesian  product  3?^  x  [— do»do].  Each  a-slice  of  the  space 
for  a  particular  a  is  a  configuration  space  with  the  obstacles  A  and  B  instantiated 
at  distance  a  apart.  Fig.  15  is  such  a  slice. 

More  generally,  suppose  we  have  a  configuration  space  C  for  the  degrees  of 
freedom  of  the  moving  object.  Let  J  be  an  arbitrary  index  set  which  parameterizes 
the  model  error.  (Above,  J  was  [—do,  do]).  Then  the  generalized  configuration  space 
with  model  error  is  C  x  J.  One  way  to  think  of  this  construction  is  to  imagine  a 
collection  of  possible  “universes”,  {  Ca  }  for  a  in  J.  Each  Ca  is  a  configuration  space, 
containing  configuration  space  obstacles.  The  ambient  space  for  each  Cq  is  some 
canonical  C.  C  x  J  is  simply  the  natural  product  representing  the  ambient  space  of 
their  disjoint  union.  There  is  no  constraint  that  J  be  finite  or  even  countable.  In  fig. 
3,  C  is  again  the  cartesian  plane,  and  d  is  a  three-dimensional  product  space.  One 
of  the  J  dimensions  is  circular,  to  parameterize  the  angular  variation  represented 
by  as. 

In  fig.  16  we  show  the  generalized  configmation  space  for  example  (1).  Note 
that  the  goal  in  generalized  configuration  space  becomes  a  2-dimensional  surface, 


Fig.  14.  The  goal  U  to  bring  the  robot  into  contact  with  the  right  vertical 
surface  of  A.  (For  example,  the  “robot”  could  be  a  gripper  finger).  There  is 
position  sensing  uncertainty,  so  in  the  start  position  the  robot  is  only  known 
to  lie  within  some  uncertainty  ball.  There  is  also  control  uncertainty  in  the 
commanded  velocity  to  the  robot.  It  is  represented  as  a  cone,  as  shown. 


and  the  obstacles  are  S-dimensional  polyhedra.  Note  that  the  goal  siirface  vanishes 
where  A  and  B  meet. 

Given  a  configxiration  space  corresponding  to  a  physical  situation,  it  is  well 
known  how  to  represent  motions,  forces,  velocities,  and  so  forth  in  it  (eg.,  see 
[Arnold]).  The  representations  for  classical  mechanics  exploit  the  geometry  of  dif¬ 
ferentiable  manifolds.  We  mxist  develop  a  similar  representation  to  plan  motions, 
forces,  amd  velocities  in  generalized  configuration  space  .  Henceforth,  we  will  denote 
the  generalized  configuration  space  C  x  J  hy  Q.  We  develop  the  following  “axioms” 
for  “physics”  in  Q. 

(1)  At  execution  time,  the  robot  finds  itself  in  a  particular  slice  of  (although  it 
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Fig.  15.  The  equivalent  problem  in  configuration  space.  The  blocks  A  and 
the  distance  between  the  blocks  a,  and  the  commanded  velocity  v$  =  vq  with 
control  error  cone  fiec(vo).  The  position  of  >1  is  fixed. 

may  not  know  which).  Thus  we  say  there  is  only  one  ‘'real”  universe,  oq  in^ 
J.  This  ao  is  fixed.  However,  oq  b  not  known  a  priori.  Thus  all  motions  are 
confined  to  a  particular  (unknown)  oo-slice,  such  as  fig.  15.  Thb  b  because 
motions  cannot  move  between  universes.  In  fig.  16,  any  legal  motion  in  ^  b 
everywhere  orthogonal  to  the  7-axb  and  parallel  to  the  x-y  plane. 

(2)  Suppose  in  any  a-slice  the  position  sensing  uncertainty  ball  about  a  given 
sensed  position  b  some  set  The  set  i2  in  fig.  15  b  such  a  ball.  We  csumot 
sense  across  J:  position  sensing  uncertainty  b  infinite  in  the  J  dimensions.^ 
Thiis  the  position  sensing  imcertainty  in  is  the  cylinder  Btp  x  J.  In  figs. 
15,16,  thb  simply  says  that  x  and  y  are  known  to  some  precbion,  while  a  is 
unknown.  The  initial  position  in  fig.  15  b  given  by  J?  x  [—do,  do].  Thb  cylinder 


^  ao  is  a  point  in  the  multi-dimensional  space  J. 

^One  generalization  of  the  framework  would  permit  and  plan  for  sensing  in  J.  In  this 
case  one  would  employ  a  bounded  sensing  uncertainty  ball  in  the  J  dimensions. 
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Fig.  16.  The  generalized  configuration  space  obstacles  for  example  (1).  The 
generaJized  configuration  space  is  three  dimensional,  having  x  and  y  degrees  of 
motion  freedom,  and  an  a  degree  of  model  error  freedom.  Legal  motions  are 
parallel  to  the  x-y  plane,  and  orthogonal  to  the  J  axis. 


is  a  S'dimensional  solid,  orthogonal  to  the  x-y  plane  and  parallel  to  the  7-axis 
in  fig.  16. 

(3)  Suppose  in  the  configuration  space  C,  the  velocity  control  uncertainty  about  a 
given  nominal  conunanded  velocity  is  a  cone  of  velocities  Bgc-  Such  a  cone  is 
shown  in  fig  15.  This  cone  lies  in  the  phase-space  for  C,  denoted  TC.  (Phase 
space  is  simply  Position-space  x  Velocity-space.  A  point  in  phase  space  has 
the  form  (x,  v),  and  denotes  an  instantaneous  velocity  of  v  at  configuration 
x).  Phase  space  represents  all  possible  velocities  at  all  points  in  C  The  phase 
space  for  Q  is  obtained  by  indexing  TC  by  7  to  obtain  TC  x  7.  All  velocities 
in  generalized  configuration  space  lie  in  TC  x  7.  For  Ex.  (1)  TC  x  7  is 
X  [— The  generalized  velocity  uncertainty  cones  are  two-dimensional, 
parallel  to  the  x-y  plane,  and  orthogonal  to  the  7  axis. 
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(4)  Generalized  damper  dynamics  extend  straight-forwardly  to  so  motions  sat¬ 
isfy  /  =  B{y  —  uo)  where  /,  u,  and  uo  lie  in  TC  x  J.  Thus  friction  cones  from 
configuration  space  (see  [Erdmann])  naturally  embed  like  generalized  velocity 
cones  in  TC  x  J. 

These  axioms  give  an  intuitive  description  of  the  physics  of  Q.  A  formal  axiom- 
atization  is  given  in  an  appendix.  We  have  captured  the  physics  of  ^  using  a  set  of 
generalized  uncertainties,  friction,  and  control  chairacteristics  (1-4).  These  axioms 
completely  characterize  the  behavior  of  motions  in 

2.3.  Representing  Pushing  Operations  in  Generalized  Configuration 
Space 

By  relaxing  axiom  (1),  above,  we  can  consider  a  generalization  of  the  model 
error  framework,  in  which  pushing  motions  are  permitted,  as  well  as  compliant 
and  gross  motions.  We  relax  the  assumption  that  motion  between  universes  is 
impossible,  and  permit  certain  motions  across  J.  Consider  example  (1).  Observe 
that  a  displacement  in  J  corresponds  to  a  displacement  in  the  position  of  the  block 
B.  Thus  a  motion  in  J  should  correspond  to  a  motion  of  B.  Suppose  the  robot  can 
change  the  position  of  B  by  pushing  on  it,  that  is,  by  exerting  a  force  on  the  surface 
of  B.  The  key  point  is  that  pushing  operations  may  be  modeled  by  observing  that 
commanded  forces  to  the  robot  may  result  in  changes  in  the  environment.  That 
is,  a  commanded  force  to  the  robot  can  result  in  motion  in  C  (sliding)  as  well  as 
motion  in  J  (pushing  the  block).  Let  us  develop  this  notion  further. 

Our  previous  discussion  assumed  that  motion  across  J  was  impossible.  That  is, 
all  motion  is  confined  to  one  a-slice  of  generalized  configuration  space.  In  example 
( 1 )  this  is  equivalent  to  the  axiom  that  B  does  not  move  or  deform  under  an  applied 
force.  Such  an  axiom  makes  sense  for  apphcations  where  B  is  indeed  immovable, 
for  example,  if  A  and  B  are  machined  tabs  of  a  connected  metal  part.  However, 
suppose  that  H  is  a  block  that  can  slide  on  the  table.  See  fig.  17.  Then  an  applied 
force  on  the  surface  of  the  block  can  cause  the  block  to  slide.  This  corresponds  to 
motion  in  J.  In  general,  the  effect  of  an  applied  force  will  be  a  motion  which  slides 
or  sticks  on  the  surface  of  B,  and  which  causes  B  to  slide  or  stick  on  the  table.  This 
corresponds  to  a  coupled  motion  in  both  C  and  J.  When  the  motion  maintains 
contact,  it  is  tangent  to  a  surface  5  in  generalized  configuration  space. 

Our  goal  is  to  generalize  the  description  of  the  physics  of  Q  to  permit  a  rigorous 
account  of  such  motions.  This  model  can  then  be  employed  by  an  automated 
planner.  Such  a  planner  could  construct  motion  strategies  whose  primitives  are 
gross  motions,  compliant  motions,  and  pushing  motions.^ 

*Our  model  of  pushing  is  less  general  than  [Mason,  82],  since  it  requires  knowledge  of 
the  center  of  friction.  See  an  appendix  for  details. 


Fig.  17.  A  force  /«  applied  to  the  top  surface  of  B  can  cause  sliding  (or  sticking) 
on  the  top  of  B,  coupled  with  motion  of  B  on  the  table.  This  corresponds  to  a 
pushing  motion  in  d.  By  giving  the  right  geometric  structure  to  the  surface  5, 
we  csm  predict  the  resulting  cone  of  motions  in  Q,  given  a  commanded  velocity 
ft  subject  to  control  uncertainty.  A  planner  could  generate  a  motion  along  S  in 
order  to  plan  pushing  operations. 


The  description  of  the  physics  should  embrace  the  following  observations: 

The  phase  space  for  C  corresponds  to  forces  exerted  at  the  center  of  mass  of 
the  robot.  The  phase  space  for  J  corresponds  to  forces  acting  at  the  center  of  mass 
of  B.  When  pushing  is  allowed,  the  phase  space  for  generalized  configuration  space 
is  not  TC  X  J  but  TC  x  TJ.  In  the  pushing  application,  all  forces  are  exerted 
in  C,  but  may  be  ‘‘transferred”  to  J  via  the  contact.  In  other  words,  the  applied 
forces  we  consider  will  have  zero  component  along  J.  However,  they  may  result  in 
a  motion  in  7,  via  the  transferred  pushing  force. 

In  free  space,  or  on  surfaces  generated  by  immovable  objects,  all  differential 
motions  lie  within  one  a-slice.  This  is  because  objects  can  only  be  pushed  when 
the  robot  is  in  contact  with  them. 
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Fig.  18.  Pushing  on  the  side  of  B  can  cause  B  to  slide,  even  in  the  absence 
of  friction.  This  behavior  can  be  modeled  by  giving  the  surface  5  a  normal 
which  points  across  J.  The  surface  can  exert  reaction  forces  along  this  normal. 
Thus,  applying  a  force  in  C  results  in  a  reaction  force  with  a  J  component.  The 
resulting  motion  moves  across  J,  tangent  to  5.  That  it,  it  pushes  the  block. 
Friction  can  also  be  introduced  on  S.  A  picture  of  the  friction  cone  developed 
in  an  appendix  is  shown.  It  represents  the  range  of  reaction  forces  the  surface  S 
can  exert. 


Along  sxirfaces  generated  by  objects  that  can  be  pushed,  the  differential  motions 
are  tangent  to  the  surface  in  Q,  and  may  move  along  J  as  well  as  C.  See  fig.  17. 

A  motion  in  free  space  corresponds  to  a  gross  motion.  A  motion  on  a  surface 
staying  within  one  a-slice  corresponds  to  a  compliant  motion.  A  motion  on  a  surface 
which  moves  SM:roes  J  corresponds  to  a  pushing  motion. 

Configuration  space  surfaces  share  many  properties  with  real  space  surfaces. 
When  pushed  on,  they  push  back.  In  particular,  they  have  a  normal.  In  the  abs<  'uce 
of  friction,  they  can  exert  reaction  forces  only  along  this  normal  direction.  We 
must  define  what  the  normals  to  generalized  configuration  space  surfaces  are.  For 
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example,  see  fig.  18.  The  normal  is  transverse  to  7,  so  that  even  when  the  applied 
force  lies  exclusively  in  C,  the  surface  exerts  a  reaction  force  with  a  J  component. 
Thus  the  resultant  force  can  cause  a  motion  across  7,  tangent  to  S.  In  fig.  18  this 
implies  that  pushing  on  the  side  of  B  results  in  a  transferred  force  to  7,  causing 
B  to  slide.  In  generalized  configuration  space,  this  is  simply  viewed  as  applying  a 
force  to  a  surface  S,  which  exerts  a  reaction  force  across  7.  Since  the  resultant  force 
is  across  7,  the  motion  in  Q  will  be  in  that  direction  (under  damper  dynamics). 

The  physics  is  complicated  by  the  introduction  of  friction.  Given  2m  applied 
force,  one  of  four  qualitative  outcomes  are  possible.  (1)  The  motion  may  slide  in 
C  and  7.  This  corresponds  to  pushing  while  sliding®  at  the  point  of  contact.  (2) 
The  motion  may  stick  in  C  and  slide  in  7.^  This  corresponds  to  pushing  with  no 
relative  motion.  (3)  The  motion  may  slide  in  C  and  stick  in  7.  This  corresponds 
to  compliant  motion  in  one  a-slice.  (4)  The  motion  may  break  contact.  This 
corresponds  to  the  initiation  of  gross  motion  in  one  a-slice. 

In  order  to  generalize  physical  reasoning  to  generalized  configmation  space,  we 
must  provide  a  generalization  of  the  configuration  space  friction  cone  [Erdmann]  for 
generalized  configuration  space.  The  friction  cone  represents  the  range  of  reaction 
forces  that  a  surface  in  generalized  configuration  space  can  exert.  A  picture  of  this 
generalized  cone  is  shown  in  fig.  18.  Using  the  friction  cone,  it  is  possible  to  specify 
a  geometrical  computation  of  reaction  forces.  Such  an  algorithm  is  necessary  for 
a  plarmer  to  predict  the  possible  resulting  motions  from  an  imcertainty  cone  of 
commanded  applied  forces.  For  example,  see  fig.  17. 

By  characterizing  the  physics  of  pushing  and  sliding  via  geometrical  constraints 
in  generalized  configuration  space,  it  appears  that  a  unified  planning  framework  for 
gross-,  compliant-,  and  pushing  motions  emerges.  However,  certain  aspects  of  the 
physics  require  elaboration  and  simplification  before  a  practical  planner  for  pushing 
operations  can  be  implemented;  see  an  appendix  for  details. 

*  2.3.1  Example:  The  Sticking  Cone 

This  starred  subsection  may  be  skipped  at  first  reading. 

As  an  example  of  how  a  planner  could  reason  about  friction  in  generalized 
configuration  space,  see  fig.  19.  Here  we  take  the  configuration  spaces  of  the  robot 
and  of  B  to  be  cartesian  planes.  (See  fig.  14  to  recall  the  definition  of  the  robot 
and  B).  Assume  that  we  can  apply  a  two  dimensional  force  fc  on  the  robot,  and 
a  two  dimensional  force  fj  at  the  center  of  mass  of  B.  (This  assumption  is  for  the 
sake  of  discussion;  in  pushing  applications,  fj  would  be  zero).  The  friction  cone 

®0r  rotating. 

^This  outcome  is  not  possible  in  the  example  with  block  B,  since  if  B  moves,  this  causes 
motion  in  C  and  in  7. 


Fig.  19.  Assume  a  fixed,  negative  normal  force  at  the  center  of  mass  of  B. 

The  30  force  space  at  7  represents  the  product  of  the  2D  forces  fc  that  can  be 
exerted  by  the  robot  on  the  surface  of  fl,  with  the  ID  tangential  forces  fj  that 
can  be  exerted  at  the  center  of  mass  of  B.  An  applied  force  (/c,  fj)  in  the  cone 
represents  a  combination  of  forces  that  causes  no  motion  in  Q,  that  is,  neither 
sliding  on  the  surface  of  By  nor  of  B  on  the  table.  Note  that  the  cone  in  Q  is 
skewed  out  of  the  embedded  tangent  space  to  C  at  z.  This  is  because  when 
a  force  fe  is  applied  in  the  friction  cone  on  the  top  surface  of  B,  the  block  B 
can  slide  unless  an  opposing  force  is  exerted  tangentially  at  the  center  of  mass 
of  B.  By  intersecting  the  sticking  cone  with  the  negative  velocity  cone,  we  can 
determine  whether  sticking  is  possible  on  5. 

in  generalized  configuration  space  will  then  be  four-dimensional.  This  is  hard  to 
draw;  we  have  selected  a  fixed,  negative  normal  component  for  fj.  The  3D  force 
space  at  the  point  of  contact  T  represents  the  product  of  the  2D  forces  that  can  be 
exerted  by  the  robot  on  the  surface  of  5,  with  the  ID  tsmgential  forces  that  can  be 
applied  at  the  center  of  mass  of  B.  An  applied  force  {fcfj)  in  the  negative  of  the 
cone  in  fig.  19  represents  a  combination  of  forces  that  causes  no  motion  in  Q,  that 
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is,  neither  sliding  on  the  surface  of  B,  nor  of  B  on  the  table.  Note  that  the  cone 
in  Q  is  skewed  out  of  the  embedded  tangent  space  to  C  at  x  (denoted  T^C  in  the 
figure).  This  is  because  when  a  force  fc  is  applied  in  the  friction  cone  on  the  top 
surface  of  B,  the  block  B  can  slide  unless  an  opposing  force  is  exerted  tangentially 
at  the  center  of  mass  of  B. 

Let  us  call  the  cone  in  fig.  19  the  sticking  cone  AC.  Using  the  sticking  cone,  we 
can  now  specify  a  geometrical  computation  to  determine  when  sticking  occurs  at 
X,  assuming  generalized  damper  dynamics:  Simply  intersect  the  negative  velocity 
control  uncertainty  cone  —Bgc{vg)  with  AC.  If  the  intersection  is  trivial,  then  sticking 
cannot  occur.  If  the  intersection  is  non-trivial,  then  sticking  can  occur.  If  the 
negative  velocity  cone  lies  inside  AC,  then  sticking  must  occur. 

This  shows  that  the  computation  to  determine  whether  sticking  is  possible  at 
a  point  reduces  to  simple  geometric  cone  intersection. 

Now  we  return  to  the  pushing  application,  by  restricting  the  applied  force  fj 
in  J  to  be  zero.  See  fig.  19.  Assume  it  is  impossible  to  apply  force  at  the  center 
of  mass  of  B.  Therefore,  the  velocity  cone  is  two  dimensional  and  lies  entirely  in 
the  tangent  space  to  C  at  x;  it  has  no  J  component.  This  two-dimensional  cone  is 
intersected  with  the  3D  cone  AC  to  determine  whether  sticking  is  possible  at  x. 

Let  us  emphasize  that  by  insisting  that  the  force  fj  applied  in  J  be  zero,  we 
obtain  a  two-dimensional  control  uncertainty  cone,  even  though  generalized  config¬ 
uration  space  has  four  degrees  of  freedom.  Thus,  in  the  model  error  framework,  the 
generalized  control  uncertainty  can  be  viewed  as  a  non-holonomic  constraint.  Holo- 
nomic  constraints  are  constraints  on  the  degrees  of  freedom  of  the  moving  object(s); 
non-holonomic  constraints  are  constraints  on  their  differential  motions.  Holonomic 
constraints  can  be  captured  by  surfaces  in  (generalized)  configmation  space.  To 
capture  non-holonomic  constraints  geometrically,  we  must  introduce  constraints  in 
the  phase  space.  This  viewpoint  is  developed  in  an  appendix,  where  we  provide  a 
more  rigorous  account  of  the  construction  of  normals,  friction  cones,  sticking  cones, 
and  the  computation  of  reaction  forces  in  generalized  configuration  space. 
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2.4.  Guaranteed  Plans  in  Generalized  Configuration  Space 

A  motion  strategy  [LMT]  is  a  commanded  velocity  (such  as  Vg  in  fig.  15) 
together  with  a  termination  predicate  which  monitors  the  sensors  and  decides  when 
the  motion  has  achieved  the  goal.  Given  a  goal  G  in  configuration  space,  we  can 
form  its  preimage  [LMT].  The  preimage  of  G  is  the  region  in  configuration  space 
from  which  all  motions  are  guaranteed  to  move  into  G  in  such  a  way  that  the  entry 
is  recognizable.  That  is,  the  preimage  is  the  set  of  all  positions  from  which  all 
possible  trajectories  consistent  with  the  control  uncertainty  are  guaranteed  to  reach 
G  recognizably.  For  example,  see  fig.  20.  The  entry  is  recognized  by  monitoring 
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the  position  and  velocity  sensors  until  the  goal  is  attained.  Fig.  20  is  a  directional 
preimage:  only  one  commanded  velocity  uj  is  considered.  Here  all  preimage  points 
reach  the  goal  recognizably  under  this  particular  vj.  The  non- directional  preimage 
is  the  union  of  all  directional  preimages. 

We  envision  a  back-chaining  planner  which  recursively  computes  pre-images 
of  a  goal  region.  Successive  subgoals  are  attained  by  motion  strategies.  Each 
motion  terminates  when  all  sensor  interpretations  indicate  that  the  robot  must  be 
within  the  subgoal.  [LMT,E]  provide  a  formal  framework  for  computing  preimages 
where  there  is  sensing  and  control  uncertainty,  but  no  model  error.  In  paulicular, 
[Erdmann)  shows  how  hackprojections  may  be  used  to  approximate  preimages.  The 
backprojection  of  a  goad  G  (with  respect  to  a  commanded  velocity  uj)  con.^icts  of 
those  positions  guaranteed  to  enter  the  goal  (under  uj).  Recognizabilit>  of  the 
entry  plays  no  role.  Fig.  21  illustrates  the  difference  between  backprojerti.  n-;  and 


Fig.  21.  Here,  the  radius  of  the  position  sensing  uncertainty  ball  is  twice  the 
width  of  the  hole.  Sliding  occurs  on  all  surfaces  under  the  control  velocities 
shown.  The  preimage  of  the  goal  under  commanded  velocity  vj  is  P$(G).  The 
backprojection  B0{G)  strictly  contains  this  preimage:  while  all  points  in  the 
backprojection  are  guaranteed  to  reach  G,  the  sensing  inaccuracy  is  so  large 
that  the  termination  predicate  cannot  tell  whether  the  goal  or  the  left  hori¬ 
zontal  surface  has  been  reached.  Only  from  the  preimage  can  entry  into  G  be 
recognized. 

preimages.  Here  the  radius  of  position  sensing  uncertainty  is  greater  than  twice  the 
diameter  of  the  hole.  Sliding  occius  on  all  surfaces.  Furthermore,  we  assume  that 
the  robot  has  no  sense  of  time  (i.e.,  no  clock) — for  exsunple,  it  might  be  equipped 
with  a  contact  sensor  that  only  fires  once.  The  back  projection  Bs(Ct)  strictly 
contains  the  preimage  while  all  points  in  the  backprojection  are  guaranteed 

to  reach  G,  the  sensing  inaccuracy  is  so  large  that  the  termination  predicate  cannot 
tell  whether  the  goal  or  the  left  horizontal  surface  has  been  reached.  Only  from  the 
preimage  can  entry  into  G  be  recognized. 

Preimages  provide  a  way  to  construct  guaranteed  plans  for  the  sit\iation  with 


no  model  error.  Can  preimages  and  backprojections  be  generalized  to  situations 
with  model  error?  The  answer  is  yes.  Consider  fig.  15,16.  The  goal  in  generalized 
configuration  space  is  the  surface  G  (which  has  two  components).  The  start  region 
is  the  cylinder  R  x  J  (where  J  is  [—do, do]).  The  generalized  control  and  sensing 
uncertainties  in  Q  are  given  by  the  physics  axioms  above.  These  xmcertainties 
completely  determine  how  motions  in  generalized  configuration  space  must  behave. 
We  form  the  backprojection  of  G  under  these  uncertainties.  The  backprojection  has 
two  components,  shown  in  fig.  22,23.  It  is  a  three-dimensional  region  in  ^  of  all 
triples  (x,  y,  a)  that  are  guaranteed  to  reach  G  under  the  control  imcertainty  shown 
in  fig.  15.  Equivalently,  we  can  view  it  as  all  points  in  Q  guaranteed  to  reach  G  under 
the  generalized  uncertainties  that  specify  Q's  physics.  Note  that  backprojerti<,iis  do 
not  “converge  to  a  point”  along  the  J  axis  (compare  fig.  20).  This  is  because  there 
is  perfect  control  along  J,  and  the  commanded  velocity  along  J  is  zero.  This  is  why 


Fig.  23.  The  backprojection  of  the  other  component  of  G.  A  typical  O'sHce  for 
a  negative  U  shown  below.  The  backprojection  in  G  of  the  entire  goal  surface  is 
the  union  of  the  backprojections  shown  in  figs.  22,23. 


in  this  particular  G  there  are  two  disjoint  backprojection  regions,  one  from  each 
component  of  G.  Furthermore,  recursively-computed  backprojections  can  never 
cover — or  even  intersect — any  slice  of  G  in  which  the  goal  vanishes. 

The  trick  here  was  to  view  the  motion  planning  problem  with  n  degrees  of 
motion  freedom  and  k  degrees  of  model  error  freedom  as  a  planning  problem  in 
an  (n  k)-dimensional  generalized  configuration  space,  endowed  with  the  special 
physics  described  above.  The  physics  is  characterized  precisely  by  axioms  defining 
certain  special  sensing  and  control  uncertainties  in  G-  The  definitions  and  results  for 
pre-images  and  backprojections  [LMT,E]  in  configuration  space  generalize  muiatis 
mutandis  to  G  endowed  with  this  physics;  this  is  proved  in  an  appendix(A.3).  Thus 
our  frsunework  reduces  the  problem  of  constructing  guaranteed  motion  strategies 
with  model  error  to  computing  preimages  in  a  somewhat  more  complicated,  nnd 
higher -dimensional  configuration  space. 


In  this  example,  because  the  position  of  B  varies  linearly  with  a,  the  surfaces 
in  Q  are  planar  and  the  generalized  configuration  space  obstacles  are  polyhedral. 
Below,  in  sec.  6,  we  give  polynomial-time  algorithms  for  computing  these  backpro- 
jections.  While  they  have  been  computed  by  hand  here,  note  that  this  reduction 
gives  us  an  efiicient  planning  algorithm  for  an  important  special  case. 
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3.  Error  Detection  and  Recovery 


If  we  were  exclusively  interested  in  constructing  guaranteed  motion  strategies 
in  the  presence  of  model  error,  we  would  be  done  defining  the  framework:  having 
reduced  the  problem  to  computing  preimages  in  we  could  now  turn  to  the  im¬ 
portant  and  difficult  problems  of  computing  and  constructing  Q,  and  further  extend 
the  work  of  [LMT,E]  on  computing  preimages  in  general  configuration  spaces. 

However,  guaranteed  strategies  do  not  always  exist.  In  example  (1),  (figs.  14- 
16)  there  is  no  guaranteed  strategy  for  achieving  the  goal,  since  the  goal  may  vanish 
for  some  values  of  a.  Because  tolerances  may  cause  gross  topological  changes  in 
configuration  space,  this  problem  is  particularly  prevalent  in  the  presence  of  model 
error.  In  the  peg-in-hole  problem  with  model  error  (figs.  3-13)  the  goal  may  also 
vanish  (the  hole  may  close  up)  for  certain  regions  in  J.  More  generally,  there  may 
be  values  of  a  for  which  the  goal  may  still  exist,  but  it  may  not  be  reachable. 
For  example,  in  a  variant  of  the  problem  in  fig.  3,  an  obstacle  could  block  the 
chaxmel  to  the  goal.  Then  G  is  non-empty,  but  also  not  reachable.  Finally,  and 
most  generally,  there  may  be  values  of  a  for  which  the  goal  is  reachable  but  not 
recognizably  reachable.  In  this  case  we  still  cannot  guarantee  plans,  since  a  planner 
cannot  know  when  they  have  succeeded. 

These  problems  may  occur  even  in  the  absence  of  model  error.  However,  with¬ 
out  model  error  a  guaranteed  plan  is  often  obtainable  by  back-chaining  and  adding 
more  steps  to  the  plan.  In  the  presence  of  model  error  this  technique  frequently  fails: 
in  example  (1),  no  chain  of  recursively-computed  preimages  can  ever  cover  the  start 
region  R  x  J.  The  failtire  is  due  to  the  peculiar  sensing  and  control  characteristics 
(1-4)  in  generalized  configuration  space  . 

In  response,  we  will  develop  Error  Detection  and  Recovery  (EDR)  strategies. 
These  are  characterized  as  follows: 

•  An  EDR  strategy  should  attain  the  goal  when  it  is  recognizably  reachable,  and 
signal  failure  when  it  is  not. 

•  It  should  also  permit  serendipitous  achievement  of  the  goal. 

•  Furthermore,  no  motion  guaranteed  to  terminate  recognizably  in  the  goal 
should  ever  be  prematurely  terminated  ais  a  failure. 

•  Finally,  no  motion  should  be  terminated  eis  a  failure  while  there  is  any  chance 
that  it  might  serendipitously  achieve  the  goal  due  to  fortuitous  sensing  and 
control  events. 

These  are  called  the  “EDR  Axioms”,  they  will  be  our  guiding  principles.  Can 
we  construct  such  strategies?  The  answer  is,  basically,  yes.  Let  us  construct  one  for 
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Fig.  24.  A  typiod  a-slice  of  the  forward  projection  of  the  “bad”  region.  The 
forward  projection  U  the  region  F.  a  is  negative  and  almost  zero.  is  an  EDR 
region  in  the  forward  projection. 


a  variant  of  example  (1).  We  first  restrict  our  attention  to  the  environments  where 
a  lies  in  the  interval  [di ,  do]  where  di  is  small  and  negative.^ 

Call  the  start  region  U  =  RxJ.  The  strategy  of  example  (1)  commands  velocity 
Vq  (fig  15).  It  tries  to  terminate  the  motion  in  G  by  detecting  sticking.  Call  this 
strategy  $.  We  will  use  0  as  a  starting  point,  and  try  to  build  an  EDR  strategy 
from  it.  Now,  U  is  divided  into  a  “good”  region,  from  which  9  is  guaranteed,  and  a 
“bad”  region,  from  which  it  is  not.  The  goal  vanishes  for  the  bad  region.  We  wish 
to  extend  0  to  an  EDR  strategy  from  all  of  U. 

Let  xis  investigate  the  result  of  executing  9  from  the  “bad”  region.  We  employ 
the  forward  projection  [Erdmann].  The  forward  projection  of  a  set  V  under  ^  is  all 

^See  hg.  15.  Formally,  if  w  is  the  width  of  A  and  B,  and  c  is  the  position  sensing 
uncertainty,  then  |di|  <  min(u;,  j).  Otherwise,  if  a  can  be  arbitrary,  no  strategy  can 
always  distinguish  the  right  edge  of  A  from  the  right  edge  of  Bl 
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configurations^  which  are  possibly  reachable  from  V  under  Uq  (subject  to  control 
uncertainty).  It  is  denoted  Fg{V).  Forward  projections  only  address  reachability: 
the  termination  predicate  is  ignored  and  only  the  control  uncertainty  bound  and 
commanded  velocity  Vq  are  needed  to  specify  the  forward  projection. 

Fig.  24  shows  a  typical  a-slice  of  the  forward  projection  of  the  “bad”  region. 
The  goal  vanishes  in  this  slice;  the  dashed  line  indicates  where  the  goal  would 
be  in  other  slices.  We  can  now  define  an  EDR  strategy  as  follows.  Consider  the 
region  H  in  fig.  24.  The  termination  predicate  can  distinguish  between  G  and 
H  bzised  on  position  sensing,  velocity  sensing,  or  elapsed  time.*  Consider  H  as  a. 
two-dimensional  region  in  just  a  slice  of  it  is  shown  in  fig.  24.  Note  that  in 
this  example,  H  only  exists  in  the  slices  in  which  G  vanishes.  Thus  the  motion  is 
guaranteed  to  terminate  recognizably  in  G  iff  the  motion  originated  in  the  “good” 
region  of  U.  Otherwise  the  motion  terminates  recognizably  in  H.  In  the  first  case, 
the  termination  predicate  signals  success,  in  the  latter,  failure. 

Clearly  this  EDR  strategy  satisfies  the  “EDR  ^lxioms”  above.  The  problem 
of  constructing  EDR  strategies  may  be  attacked  as  follows:  We  take  a  strategy  6 
as  data.  Next,  an  EDR  region  H  is  found.  H  is  introduced  as  a  “bad  goal”,  and 
a  strategy  is  found  which  achieves  either  G  or  H  (subject  to  the  EDR  axioms). 
Finally,  we  must  not  only  recognize  that  G  ox  H  hats  been  attained,  but  also  know 
■which  goal  has  been  reached. 

Now,  think  of  d  as  indexing  the  “angular  direction”  of  the  commanded  veloc¬ 
ity.  By  quantifying  over  all  $,  we  can  in  principle  define  “non-directional”  EDR 
strategies.  This  problem  is  similar  to  constructing  non-directional  preimages.  For 
now,  we  restrict  our  attention  to  one-step  plans.  Later,  we  consider  n-step  plans. 

3.1.  Generalizing  the  Construction 

We  now  present  an  informal  account  of  how  the  construction  of  EDR  regions 
and  strategies  may  be  generalized.  Do  not  be  alarmed  if  some  of  our  examples 
are  without  model  error.  Since  we  have  reduced  the  planning  problem  with  model 
error  to  planning  in  a  (different)  configuration  space,  it  suffices  to  consider  general 
configuration  spaw:es  in  this  discussion. 

So  far  the  preimages  we  have  considered  are  strong  preimages,  in  that  all  possi¬ 
ble  motions  are  guaranteed  to  terminate  recognizably  in  the  goal.  The  weak  preim¬ 
age  [LMT]  (with  respect  to  a  commanded  velocity)  is  the  set  of  points  which  could 
possibl]/  enter  the  goal  recognizably,  given  fortuitous  sensing  and  control  events.  See 
fig.  25.  We  will  use  the  weak  preimage  to  capture  the  notion  of  serendipity  in  the 

^Actually,  forward  projections  are  in  phase-space,  so  this  is  the  position  component  of 
the  forward  projection. 

*Given  the  sensing  uncertainties  of  example  (1). 
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EDR  axioms.  The  idea  is  that  a  motion  may  be  terminated  in  failure  as  soon  as 
egress  &om  the  weak  preimage  is  recognized. 

Now  consider  fig.  26.  Assume  sliding  occurs  on  the  vertical  edges,  and  sticking 
on  the  horizontal  ones.  The  (strong)  preimage  of  the  goal  G  is  denoted  P.  A 
motion  strategy  $  with  commanded  velocity  vj  is  guaranteed  for  the  region  JV,  but 
the  starting  region  is  the  larger^  B,  The  weak  preimage  of  G  is  denoted  P.  The 
forward  projection  of  the  “bad”  region  i?  —  P'  is  P#(P  —  P').  In  fig.  26,  it  is  in 
fact  equal  to  Pt(Ii).  Using  0  as  data,  how  can  we  construct  an  EDR  strategy  that 
is  applicable  for  all  of  P?  Let  us  first  try  taking  the  EDR  region  H  =  Hq,  where 
Hq  is  the  set  difference  of  the  forward  projection  of  the  “bad”  region  and  the  weak 
prdmage: 


Ho  =  Ptf(P  -  P')  -  P. 


*Note  that  in  general,  R  and  P'  need  not  be  cylinders,  but  can  be  arbitrary  subsets  of 

G. 


L  «  .  i  '  / 


Bm(\  ) 


I 

'I 


Fig.  26.  R  U  the  st&rt  region.  P  is  the  strong  preimage  of  G.  R'  is  the  region  in 
R  from  which  the  strategy  is  guaranteed  to  reach  G  recognizably.  P  is  the  weak 
preimage.  H  is  the  forward  projection  of  R  outside  the  weak  preimage.  It  is  the 
EDR  region. 


If  we  can  distinguish  between  G  and  H,  then  if  is  a  good  EDR  region,  and  we  have 
constructed  an  EDR  strategy. 


Taking  H  =  Hq  above  is  not  sufficiently  general.  Consider  fig.  27.  It  is 
possible  for  a  motion  from  R  to  stick  forever  in  the  region  H,,  which  is  within  the 
weak  preimage.  However,  a  motion  through  H,  is  not  guaranteed  to  stick  in  H,\ 
it  may  eventually  slide  into  the  goal.  We  want  sliding  motions  to  pass  through  H, 
unmolested,  while  the  termination  predicate  should  halt  sticking  motions  in  H,. 

The  EDR  region  H  region  should  include  Hq.  But  it  should  also  include  ff«, 
when  sticking  occurs.  In  other  words,  H  should  include  Ha  for  all  velocities,  but 
should  only  include  H,  for  sticking  velocities  (that  is,  zero  velocities).  To  handle 
this  idea  we  introduce  simple  velocity  goals,  as  well  as  position  goals.  The  position 
and  velocity  goals  are  regions  in  phase  space. 
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Fig.  27.  Ho  in  eq.  (1)  is  not  the  entire  EDR  region.  Sticking  may  occur  within 
the  weak  preimage  in  H,.  The  EDR  region  must  include  Ho  for  all  possible 
velocities,  and  H$  for  “sticking  velocities.” 


A  goal  in  phase  space  is  a  region  in  Position-space  x  Velocity-space.  A  phase 
space  goal  is  attained  when  the  actual  position  and  velocity  can  be  guaranteed  to 
lie  in  the  region.  Let  us  constnict  the  phase-space  EDR  region  H.  If  z  is  in  Ho-, 
then  for  any  velocity  v  at  z,  (z,  u)  must  be  in  H.  Let  Tr~^{Ho)  denote  all  such  (z,  v) 
in  phase-space. 

Now,  Hg  is  the  set  of  all  points  z  in  the  weak  but  not  strong  preimage,  such 
that  sticking  can  occur  at  z.®  We  wish  to  distinguish  the  sticking  velocities  in  H,. 
Under  generalized  damper  dynamics,  these  we  essentially  the  zero  velocities.  Let 
Z{H,)  denote  the  zero  velocities  over  Hg,  that  is,  the  set  of  pairs  (z,0)  for  z  in  Hg. 
This  set  is  in  phase  space.  Then  we  see  that  Z{Hg)  is  silso  in  the  phase  space  EDR 
region  H.  Thus  H  is  the  union  of  the  sticking  velocities  over  Hg,  and  all  velocities 
over  the  forward  projection  outside  the  weak  preimage: 


^[Erdmann]  shows  how  to  decide  whether  x  €  H$  using  configuration  space  friction  cones. 


Fig.  28.  The  weak  but  not  strong  preimage  P  —  P,  from  fig.  27.  Can  a  motion 
from  R  remain  \n  P  —  P  forever?  One  way  this  may  happen  is  by  sticking  in 
H$.  In  general,  however,  there  are  other  ways. 


H  =  (2) 

To  use  H  as  an  EDR  region,  we  must  now  ensure  that  H  and  the  cylinder  over 
G  are  distinguishable  goals.  In  an  appendix,  we  show  that  if  the  strong  preimage  is 
known,  the  definition  of  (phase  space)  EDR  regions  is  constructive  up  to  reachability. 
By  this  we  mean  that  when  backprojections,  set  intersections  and  differences,  and 
friction  cones  can  be  computed,  then  so  can  H.  With  H  in  hand,  we  add  the 
recognizability  constraint  to  obtain  an  EDR  strategy. 

The  structure  of  the  “weak  but  not  strong  preimage,”  P  —  P  suggests  a  number 
of  implementation  issues.  Consider  figs.  27,28  once  more.  Suppose  we  have  a 
trajectory  originating  in  R,  subject  to  the  control  uncertainty  shown.  We  do  not 
wish  to  terminate  the  motion  while  it  remains  in  the  weak  preimage,  since  fortuitous 
sensing  and  control  events  could  still  force  recognizable  termination  in  G.  Howev.  i . 


we  can  terminate  the  motion  as  soon  as  we  recognize  egress  from  the  weak  preimage. 
This  is  why  the  forward  projection  outside  the  weak  preimage  is  contained  in  the 
EDR  region. 

As  we  have  seen,  however,  it  is  possible  for  a  trajectory  to  remain  within  the 
weak  but  not  strong  preimage  forever.  For  example,  it  can  stick  in  H,  forever.  To 
handle  this  case,  we  introduced  phase  space  EDR  goals. 

There  are  other  conditions  under  which  a  trajectory  could  stay  in  P— P  forever: 
(a)  if  the  environment  is  infinite,  or  P  — P  is  unbounded,  (b)  The  trajectory  “loops” 
in  P  —  P  forever,  (a)  and  (b)  are  qualitatively  different  from  the  case  of  sticking 
forever  in  P,,  because  they  require  motion  for  infinitely  long.  In  practice  this  may 
be  handled  by  terminating  the  motion  in  P  —  P  after  a  certain  elapsed  time.  We 
can  model  this  case  by  constructing  termination  predicates  which  “time-out.”  In 
fact,  this  “solution”  works  for  sticking  in  P,  also. 

An  alternative  is  to  extend  our  earlier  zero-velocity  analysis  to  all  of  P  —  P. 
That  is,  we  terminate  the  motion  in  the  weak  but  not  strong  preimage  when  the 
actual  velocity  is  (close  to)  zero.  It  seems  that  time-out  termination  predicates 
and/or  velocity  thresholding  must  be  used  to  solve  the  looping  problem.  Both 
solutions  seem  inelegant;  the  issue  is  subtle  and  is  addressed  further  in  a  later 
section. 
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Fig.  29.  There  are  two  possible  universes;  the  goal  G  exists  in  the  first  but 
not  the  second.  The  start  region  is  U  Aa.  Motion  0  is  guaranteed  to  move 
from  Ri  into  Si .  Motion  is  guaranteed  to  move  from  Si  into  /.  There  is  an 
8-step  plan  achieving  G  from  /Zi.  The  forward  projections  of  Ri  and  R2  are 
indistinguishable.  There  exists  no  one-step  EDR  strategy  from  the  motion  8. 


4.  Generalization  to  n-Step  EDR  Strategies 


4.1.  The  **Twin  Universe”  Example 


Example  (S).  So  far  we  have  only  considered  one-step  EDR  strategies.  We 
now  generalize  the  construction  to  n-step  strategies.  Consider  fig.  29.  Here  there 
are  two  possible  universes,  both  in  the  plane,  so  7  is  the  two  element  discrete  set, 
{  1,2}.  The  start  region  is  the  union  of  Ri  in  universe  1,  and  R2  in  universe  2.  The 
goal  exists  in  universe  1  but  not  in  universe  2.  There  is  no  one-step  EDR  strategy 
which,  from  the  start  region,  can  guarzmtee  to  achieve  G  or  recognize  that  we  are  in 
universe  2.  In  particular,  there  is  no  one-step  EDR  strategy  which  can  be  derived 
from  the  motion  vj. 


There  is  an  8-step  plan  in  universe  1  which  recognizably  achieves  G  from  start 
region  Ry.  It  is  obtained  by  back-chaining  preimages  in  universe  1.  The  plan  moves 
from  Ri  to  the  region  Si  under  uj.  Then  it  slides  along  the  top  surface  to  vertex  /, 
and  then  to  the  successive  vertex  subgoals  e  through  a,  and  finally  into  G.  We  can 
construct  a  2-step  EDR  strategy,  from  this  plzm.  First,  we  execute  motion  9  from 
the  union  of  Ri  and  R2.  This  achieves  a  motion  into  Si  in  universe  1,  or  into  S2  in 
universe  2.  The  termination  predicate  cannot  distinguish  which  has  been  attained. 
Suppose  the  second  motion  in  the  8-step  plain  is  (see  fig.  29),  and  is  guaranteed 
to  achieve  the  vertex  subgoal  /  from  start  region  Si .  We  will  try  to  construct  an 
EDR  strategy  out  of  this  second  motion.  Take  as  data;  the  subgoal  /,  the  start 
region  Si  U  S2,  the  “southwest”  motion  and  the  preimage  of  /  under  tp.  The 
EDR  region  for  these  data  is  the  forward  projection  of  S2  under  xp  (see  fig.  30). 
Presumably  this  EDR  region  is  distinguishable  from  /,  and  so  we  have  constructed 
an  EDR  strategy  at  the  second  step.  After  executing  the  second  step,  we  either 
terminate  the  plan  as  a  failure,  or  proceed  to  vertex  e,  and  eventually  to  the  goal. 

There  is  a  subtle  issue  of  where  to  terminate  the  motion  within  the  forward  pro¬ 
jection  of  Ri  UR2;  this  “where”  is  Si  US2  here,  and  is  called  the  push-forward.  Since 
they  address  termination,  push-forwards  are  to  forward  projections  as  preimages  are 
to  backprojections.  In  chapter  III  and  an  appendix,  they  are  defined  formally  and 
the  n-step  EDR  construction  is  given  in  detail. 
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Fig.  30.  The  forward  projection  under  V'  of  5}. 


5.  What  is  “Recovery”? 

So  far,  we  have  taken  a  “radical”  view  with  respect  to  “Recovery.”  We  assume 
that  in  planning  for  error  and  recovery,  one  essentially  specifies  the  maximum  length 
plan  one  is  willing  to  contemplate.  The  EDR  planner  considers  the  class  of  n-step 
strategies  and  tries  to  formulate  a  plan  that  will  achieve  the  goal  given  the  sensing, 
control,  and  model  imcertainty.^  Perhaps  such  a  plan  can  be  guaranteed.  If  not, 
then  termination  in  an  EDR  region  would  signal  failure.  This  means  that  there  was 
no  guaranteed  n-step  plan.  (A  third  possibility  is  that  G  and  H  are  never  distin¬ 
guishable  at  any  of  the  n  steps.  This  means  that  there  is  no  EDR  plan).  If  there 
is  no  guaranteed  n-step  plan,  and  some  EDR  region  H  is  recognizably  achieved, 
the  recovery  action  might  then  be  “give  up,”  or  “try  again,  using  up  the  remaining 

^Of  course,  one  could  in  principle  search  for  strategies  of  increasing  length  by  quantifying 
over  n.  At  any  one  time,  however,  one  would  reduce  to  the  case  described  here  and 
iterate. 
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number  of  steps  in  the  plan,’  if  we  &ie  serious  in  refusing  to  contemplate  plans 
longer  th2m  n  steps.  As  a  corollarj',  the  only  “Error.'’  then,  is  “being  in  the  wrong 
universe.”  or  more  accurately,  “being  in  the  wrong  start  region. "  This  viewpoint 
is  a  consequence  of  trying  to  address  EDR  and  completeness  simultaneously  More 
concretely,  suppose  we  consider  some  sensory-control-geometric  event  to  be  an  “er 
ror,”  make  a  plan  to  detect  it,  and  a  recovery  plan  in  case  it  is  detected.  If  the 
plan  can  be  giiaranteed,  then  it  can  be  found  using  [LMT].  In  this  case  the  ‘error" 
is  no  longer  an  error,  but  simply  an  “event”  which  triggers  a  conditional  branch 
of  the  plan.^  If  the  plan  cannot  be  guaranteed,  then  we  have  prop<T«ed  the  EDR 
framework,  which  allows  us  to  try  it  anyway.  If  it  fails,  however,  the  only  obvious 
recovery  action  entails  the  recursive  construction  of  EDR  sub-plans  i  see  below  i  It 
is  not  clear  what  other  kinds  of  recovery  could  be  attempted  without  exploifiiig 
addirional  knowledge:  the  recovery  branches  have  already  been  tne<l.  The  issue  us 
subtle,  and  deserves  further  attention. 

We  give  one  example  which  highlights  the  complexity  of  the  recovery  problem. 
Suppose  that  we  consider  the  class  of  4-step  plans  Given  a  4-step  plan  as  (lata, 
suppose  we  construct  a  multi-step  EDR  strategy  which  pushes  forward  on  the  first 
motion,  and  executes  an  EDR  strategy  on  the  the  second,  .\fter  executing  the 
second  motion,  we  have  recognizably  either  achievetl  the  second  subgi^al.  or  some 
EDR  region  H.  If  ff  is  achieved,  what  is  the  correct  recovery  action  ’  We  r-ould 
do  nothing,  and  signal  failure.  .Alternatively,  we  could  try  to  construct  a  plan  '  of 
length  less  than  or  equal  to  two)  to  achieve  the  goal.  Now,  if  such  a  plan  exists 
and  can  be  guaranteed,  then  the  entire  EDR  analysis  was  unne<'es,sary.  since  the 
[LMT]  framework  can  (formally)  find  such  plans.  However,  there  might  exist  a 
2-step  EDR  strategy  to  ( try  to)  achieve  the  goal  from  H  While  such  a  plan  '  tiuld 
not  be  guaranteed,  it  might  be  worth  a  try  This  suggests  that  the  failure  recovery 
action  in  an  n-step  EDR  strategy  should  be  to  recursively  construct  another  EDR 
strategy  to  achieve  the  goal  from  the  EDR  region,  using  no  more  than  the  remammit 
nimiber  of  steps.  If  n  is  1.  the  planner  shouiil  simply  signal  failure  and  stop 

EDR  is  ripe  for  probabilistic  analysis.  In  our  plans,  the  recovery  a<  tioii  is 
often,  “in  case  of  failure,  try  it  again."  Probabilistic  analysis  would  peniiit  one  to 
address  the  likelihfxid  of  success  for  uch  a  plan.  In  jiarticular.  notice  that  after  a 
failure,  it  is  frequently  the  case  that  the  run-time  extvutive  has  leameri  Homeihini; 
about  the  structure  of  the  environment.  Tlus  knowledge  is  eml>o<lie»i  m  the  siil>set 
of  J  known  to  contain  the  world  state  Even  after  achieving  an  EDR  region  and 
signalling  failure,  it  is  generally  the  case  that  the  position  in  ./  is  nior<’  po.  ;',«  b 
known  than  at  the  start  of  the  plan  execution.  Hence  we  ran  'pnte  prerisely  d'  hn» 
our  view  of  “Error  Diagnosis"  it  is  simply  th»‘  jiosition  m  generali/ed  ronfiKurat i< .n 
space,  and,  in  particular,  the  position  m  ./ 

■rriless.  r)f  course,  t)ip  error  recovery  actum  IS  lint  a  motion 


Onf  can  envision  different  sorts  of  recovery  actions.  One  type  of  recovery  would 
t)e  to  set  up  to  do  the  task  with  different  parts.  Extending  EDR  to  the  kind  of  cyclic 
activity  found  in  certain  applications  would  be  very  useful.  While  in  principle  it 
would  be  modeled  within  our  framework,  in  practice,  the  dimensionality  of  the 
generalizefl  configuration  space  would  be  prohibitive. 


6.  Implementation  and  Experiments:  One-Step  EDR 
Strategies 


In  this  section  we  describe  experiments  with  a  implemented  EDR  planner, 
called  Limited  which  is  based  on  [LMT,E,D].  The  discussion  here  focuses  on  how 
to  use  the  EDR  theory  in  a  planner  to  generate  one-step  EDR  strategies.  Later  in 
the  thesis  we  discuss  the  implementation  of  multi-step  EDR  strategies  generation. 


6.1.  Experiment:  Computing  EDR  Regions 

In  order  to  synthesize  EDR  strategies,  we  must  be  able  to  compute  EDR 
regions.  To  compute  EDR  regions,  we  must  have  tools  for  computing  forward- 
projections  and  preimages  in  generalized  configuration  space.  We  now  discuss  these 
tools  and  experiments  using  them. 

We  approximate  preimages  using  backprojections  (see  [Erdmann]).  At  present, 
the  implementation  can  compute  slice  approximations  to  EDR  re^ons  for  one-step 
plans  where  the  generalized  configuration  space  is  three-dimensional.  The  particular 
generalized  configuration  space  we  consider  is  that  of  the  gear  example  described 
in  section  1.1.  (See  fig.  2).  In  this  case,  C  is  the  cartesian  plane,  representing 
translations  of  gear  A,  and  J  is  the  2D  rotation  group  (i.e.,  a  circle),  representing 
orientations  of  the  gear  B.  The  implementation  uses  slices:  by  a  slice  we  mean 
an  a-slice  of  generalized  configuration  space  for  some  a  in  J.  a  is  the  model  error 
parameter,  and  represents  the  orientation  of  B.  We  have  implemented  an  algorithm 
which  computes  slices  of  the  three  dimensional  EDR  regions  for  both  variants  of 
the  gear  example.  In  the  first,  B  cannot  rotate,  so  no  motion  across  J  is  possible. 
In  the  second,  B  can  rotate  when  pushed,  so  motion  across  J  is  possible.  In  the 
latter  case,  backprojections  and  forward  projections  must  be  computed  across  J, 
since  it  is  possible  to  achieve  the  goal  by  moving  across  J  (rotating  B  by  pushing 
and  possibly  sliding  on  its  surface). 

Given  a  2D  slice  of  generalized  configuration  space,  LIMITED  employs  a  plane- 
sweep  algorithm  for  computing  unions,  intersections,  and  projections.  (By  projec¬ 
tions  we  mean  forward  projections,  backprojections,  and  weak-backprojections  in 
that  slice).  The  algorithm  uses  exact  (rational)  arithmetic,  and  computes  unions 
in  0((n  -t-  c)  logn)  time,  and  projections  in  O(nlogn)  time.^  The  design  and  im¬ 
plementation  of  the  2D  plane-sweep  module  is  joint  work  with  John  Canny;  the 
algorithm  is  based  on  [Neivergelt  and  Preparata]  (who  give  a  union  algorithm)  and 
related  to  [Erdmann]  (who  implemented  an  O(n^)  backprojection  algorithm,  and 
suggested  an  improved  O(nlogn)  version). 

’  Where  n  is  the  number  of  vertices  in  the  slice,  and  c  is  the  number  of  intersections. 


Fig.  31.  niustration  of  how  forward  projections  are  propagated  acroM  slices.  In 
slice  a,  the  forward  projection  of  R,  is  computed,  e  is  an  obstacle  edge 

in  Ff(R).  Configurations  on  e  correspond  to  contact  configurations  between  the 
gears.  Quasi-static  analysis  indicates  that  commanding  velocity  vj  from  e  can 
result  in  motion  in  the  -(-a  direction.  As  a  varies  in  this  direction,  e  rotates, 
sweeping  out  an  algebraic  surface  V.  V  is  followed  into  a  nearby  slice,  a',  and 
the  intersection  of  V  and  this  slice  is  e\  In  slice  a',  the  forward  projection  Fs(e') 
is  computed.  This  is  the  propagated  forward  projection. 


To  compute  projections  in  the  3D  generalized  configuration  space,  LIMITED 
propagates  projections  across  slices.  For  example,  given  a  forward  projection  in  a 
slice,  the  algorithm  finds  all  obstacle  edges  and  vertices  from  which  it  is  possible 
to  exert  a  positive  torque  on  the  obstacle  (which  is  gear  B  in  the  figures).  See 
fig.  40.  Thus  by  pushing  on  these  edges  it  is  possible  to  move  across  slices  in  the 
-ha  direction.  Each  such  edge  is  a  slice  of  an  algebraic  niled  surface  in  generalized 
configuration  space.  The  vertices  are  slices  of  algebraic  (partial)  helicoids.  Sliding 
along  the  sur£eu:e  of  B  while  causing  B  to  rotate  corresponds  to  following  the  surface 
(or  helicoid).  The  surface  is  traced  into  the  next  a-slice,  and  taken  as  a  start 
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region  from  which  to  forward-project  in  that  slice.  For  example,  see  figs.  41-42. 
The  propagated  forward  projection  must  then  be  unioned  with  propagated  forward 
projections  from  other  slices,  and  with  the  forward  projection  of  any  start  regions 
in  that  slice.  See  fig.  45.  Weak  ''"ckprojections  are  computed  analogously. 

In  order  to  compute  weak  backprojections  and  forward  projections,  we  assume 
that  there  can  be  stiction  at  the  rotation  center  of  B.  Thus  the  ratio  of  sliding 
to  turning  is  indeterminate.  In  general,  the  computation  of  strong  backprojections 
under  rotation  due  to  pushing  will  be  a  second  order  problem,  since  it  depends  on 
the  derivatives  of  this  ratio.  We  employ  a  conservative  approximation  to  the  strong 
backprojection  (namely,  the  backprojection  in  free  sp>ace  alone)  to  construct  the 
EDR  regions.  This  suffices,  since  EDR  strategies  require  only  the  weak  backpro- 
jection  and  forward  projection  (which  depend  only  on  the  possibility,  and  not  the 
velocity,  of  shding  and  turning).  Thus  there  is  a  deep  sense  in  which  EDR  strate¬ 
gies  with  model  error  seem  easier  to  compute  than  guaranteed  strategies,  because 
EDR  strategies  are  “first  order.”  This  is  consistent  with  the  intuition  that  weak 
backprojections  should  be  easier  to  compute  than  strong  backprojections. 

Figs.  32-39  show  the  EDR  regions  for  the  gear  example  (sec.  1.1)  when  no 
rotation  of  B  is  permitted.  Only  one  slice  of  ^  is  shown.  In  all  the  figures,  the  com¬ 
manded  velocity  is  “towards  the  center  of  B" ,  up  and  to  the  right.  The  magnitude 
of  the  control  error  can  be  seen  from  the  “fan-out”  in  the  forward  projection  (fig. 
35). 

Next,  we  allow  B  to  rotate.  Figs.  43-48  show  the  EDR  regions  at  four  o-slices 
of  the  3D  generalized  configuration  space.  In  this  case  motion  across  J  is  possible 
by  pushing  B,  when  B  rotates.  The  projections  have  been  propagated  across  slices 
and  unioned.  The  results  are  slices  of  the  3D  EDR  regions  across  J . 


6.2.  Experiment:  Planning  One-Step  EDR 

The  computation  of  the  EDR  regions  is  at  the  heart  of  EDR  planning.  To 
generate  one  step  EDR  strategies  LIMITED  performs  a  search  by  discretizing  the 
space  of  possible  commanded  velocities.  The  discretization  generates  a  set  of  rorn- 
manded  velcKities  to  try.  The  following  loop  is  executed  to  search  for  an  EDR 
strategy'.  Below,  we  use  the  phreise  "the  strategy  recognizably  terminates  in  G  or 
H"  to  mean  that  the  run-time  executor  can  always  distinguish  which  of  G  or  H  has 
been  achieved,  when  the  motion  terminates. 

Algorithm  lEDR 

1.  Generate  a  commanded  velocity  rj. 

2.  Compute  the  EDR  region  H  for  v^. 
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3.  Determine  whether  the  EDR  region  H  and  the  goal  G  are  distinguishable  using 
sensors.  If  so,  then  Vg  yields  a  one-step  EDR  strategy  which  recognizably  terminates 
in  G  or  H  by  monitoring  position  and  force  sensors. 

4.  Let  dG  and  dH  denote  the  set  of  obstacle  edges  within  G  and  H ,  resp.  Deter¬ 
mine  whether  the  regions  dH  and  dG  are  distinguishable  using  sensors.  If  so,  then 
Vg  yields  a  one-step  EDR  strategy  which  recognizably  terminates  on  an  obstacle  edge 
in  G  or  H .  The  termination  condition  is  contact  with^  or  sticking  on  a  surface  in 
G  or  H. 

5.  Let  push^(G)  and  push^(if)  denote  the  sticking  push-forwards.  They  are  the 
set  of  obstacle  edges  within  G  and  H,  resp.,  on  which  sticking  can  occur  under 
Vg.  Determine  whether  these  regions  are  distinguishable  using  sensors.  If  so,  then 
I'l  yields  a  one-step  EDR  strategy  which  recognizably  terminates  when  sticking  is 
detected. 

Here  is  how  LIMITED  decides  the  question.  “Are  G  and  H  distinguishable  using 

o  <1 

sensors 

H  and  G  are  distinguishable  using  position  sensing  alone  if  their  convolutions 
(Minkowski  sums)  by  the  position  sensing  error  ball  do  not  intersect. 

Each  obstacle  edge  of  H  and  G  has  an  associated  configuration  space  friction 
cone.  Two  edges  are  distinguishable  using  force  sensing  if  the  convolutions  of  their 
friction  cones  by  the  force  sensing  uncertainty  B,„  have  a  trivial  intersection.^ 

Similarly,  the  set  of  possible  sensed  reaction  forces  at  an  olistacle  vertex  ir  of 
G  or  H  may  be  found  by  taking  the  direct  sum  of  the  fnction  rones  of  the  edges 
cobounding  w.  auid  convolving  by  B^v  Again,  a  vertex  of  H  and  a  vertex  lor 
edge)  of  G  are  distinguishable  using  force  sensing  if  their  associated  cones  of  sen.sr'fi 
reaction  fo-ces  have  a  trivial  intersection. 

Limited  decides  that  G  and  H  are  distinguishable  using  position  sensing  and 
force  sensing  if  all  edges  and  vertices  of  G  and  H  are  pairwise  di.stinguishable  using 
the  position  and  force  sensing  catena  above.  The  proce<lure  works  eipiall_\  well 
for  detennining  the  distinguishaiiility  of  OCf  and  OH.  an<i  on  the  push  forward." 
push,!  |. 


Note  that  the  procfsiure  is  cornvt  for  linear  edges,  where  [lositiori  and  foree 
'eiLsing  are  separable  b^s-ause  the  s«-t  of  possilih'  reaction  forces  is  constant  along  an 
'slge  In  geiKTal,  th»*  sets  wliK'h  must  be  <iistinct  are  of  the  following  form  Let  N’  , 
be  the  convolution  of  the  friction  cone  at  s  with  the  vehnuty  s»uismg  uncertainly 
B,,.  Let  /7,pi  r  I  denote  the  position  sensing  uncertaintv  ball  tianslated  to  r  Lhen 
th«‘  set  [iGt  of  [)ossibl<  ..elisoi  1  lit  erpi  et  a  t  ions  flolli  points  in  (r  I"  dehia'd  to  be 

‘  !  1 1 1  we  VC  r,  see  see  t  loii  7  (or  a  t  ei  lin  ii  al  poi  ii  I  ■  iii  ogn  i  /  i  li  g  w  tie!  I  o' r  I  to'  I  on  1  ai  i  o  u  o  I,  ,|i 

(,  H 

*  All  interseetioM  i  oniaiiiing  onl\  the  /eto  \ f<  lor 
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/(G)  =  U  Ar(x)  X  B„(x). 
t€G 

For  general  goals  and  EDR  regions,  we  must  have  that  I{G)  and  I{H)  are  distinct. 

A  final  comment  is  required  with  regard  to  sticking  as  a  termination  condition. 
EDR  plans  generated  with  this  termination  predicate  only  make  sense  if  sticking  is 
possible  in  the  goal.  That  is,  if  sticking  is  impossible  in  the  goal,  then  all  motions 
will  terminate  in  H.  It  is  better  still  if  sticking  is  in  fact  necessary  in  the  goal.  If  this 
is  the  caise,  then  all  motions  entering  the  goal  will  terminate  in  the  goal,  whereas 
all  motions  entering  H  will  terminate  there.  For  example,  recall  the  peg-in-hole 
strategy  in  figs.  3-13,  chapter  I.  In  this  example,  sticking  is  necessary  in  the  goal 
under  both  the  first  and  the  second  motions.  For  a  motion  6,  the  formal  criterion 
that  necessary  sticking  satisfies  is 

Fs{G)  =  G. 

That  is.  if  we  view  d  as  a  “mapping”  then  G  is  a  fixed  point.  Of  course,  it  is  a 
simple  matter  to  verify  this  criterion  given  our  plane-sweep  algorithms  for  forward 
projection.  Note  also  that  there  are  other  termination  conditions  that  satisfy  the 
fixed  point  equation — one  example  is  the  “stationary  subgoals”  in  the  tray-tilting 
planner  of  [Erdmann  and  Mason]. 

Fvu-ther  details  of  the  one-step  planning  algorithms  in  LIMITED  can  be  found 
in  section  7  .\t  this  point,  however,  we  digress  to  discuss  the  complexity  of  EDR 
planning. 

6..1.  Complexity  Bounds 

We  now  give  some  complexity  bounds  for  subproblems  in  EDR  planning.  All 
our  bounds  are  upper  fiounds  The  first  question  is,  what  is  the  complexity  of  EDR 
pL'uiiiing  in  one  sli<  e'' 

Suppose  w«  are  given  a  motion  direction  rj.  and  a  planar  polygonal  envi¬ 
ronment  '  ontaming  n  edg»-s  The  environment  represents  the  configuration  space 
olistacles  in  a  planar  slice  The  c-otiHgurat ion  sjiace  obstacle  polygons  may  be  non 
<  oii\'e\  ()i|f  iiiav  not  intersfH-t  The  start  region  and  goal  are  polygons  of  (•(/iistant 
>i/e  Supnose  that  tfie  teriiiiiiat ion  <ondition  to  be  eniployetl  is  sticking  What  is 
'tie  '  iitnplexity  of  venfving  that  H  yiehls  an  EDR  strategy  in  this  slice'.' 


riieorem:  Thrrf  rri.^h  an  Oi  ahioTithm  for  drc\dinq  thr  validity  of  a  onr-.*trp 
b  l)Ii  ttratryy  trith  itK  king  trrmination  in  a  planar  ilirr 

ft  r  nions  ail'i  set  -lltferences  can  lie  coiilpUte«i  in  finie  (){{u  f  cl  logo  I  where 
'  IS  the  nunilier  of  intersei  tioiis  <  is  bi-tween  1  and  [Neivergelt  and  Piejiarata] 


**  •  ^  ‘  ' 


✓  ^  ^  ^ 
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ForwEurd  projections,  backprojections,  and  weak  backprojections  can  be  computed 
in  time  0(n  log n).  All  projections  have  size  0{n).  Determining  the  sticking  edges 
and  vertices  of  a  polygon  is  0(n).  Hence  computing  the  EDR  region  H  requires 
time  0((n  +  c)logn)  and  the  output  has  size  0{n  +  c). 

While  the  goal  G  has  constant  size,  the  algorithm  only  needs  to  distinguish  H 
from  tht  closure  of  the  free- space  goal  The  free-space  goad  is  the  set  difference  of 
G  and  the  configmration  space  obstacles,  CO.  G  —  CO  has  size  0(n). 

Determining  whether  H  and  G  —  CO  are  distinguishable  can  require  a  pairwise 
test  of  their  edges  and  vertices.  This  takes  time  0((n  +  c)n).  □ 

Now,  in  Limited,  the  input  is  given  as  a  set  of  convex,  possibly  overlapping 
read-space  polygons.  Suppose  the  input  is  given  as  real  space  polygonal  obstacles 
of  size  0{m).  That  is,  the  totad  number  of  edges  is  m.  The  configuration  space 
obstacles  must  first  be  computed.  This  can  take  time  (9(m^),  since  the  input  could 
consist  of  two  sets  of  ^  convex  (in  fact,  triangulair)  polygons  which  must  be  pairwise 
convolved.  The  output — a  set  of  overlapping  convex  configuration  sp>ace  obstacle 
polygons — has  size  0(m).  The  union  of  the  configuration  space  obstaurles  must  be 
computed.  This  requires  time  0((m  -f  c')logn)  and  the  output  hais  size  O(c').  c\ 
the  number  of  intersections,  is  between  1  and  m^.  Taking  n  to  be  c',  we  find  that 
the  complexity  of  EDR  verification  in  a  planar  sUce  is  <  ((y)^)  ,  or  0(m®),  when 
measured  in  the  size  of  the  real  space  input. 

These  bounds  are  not  for  an  ide2dized  form  of  the  one-step  EDR  algorithm, 
but  rather  for  a  full,  exact-arithmetic  implementation.  In  practice — by  which  we 
mean  for  the  experiments  in  this  thesis — we  found  that  while  c',  the  complexity  of 
the  configuration  space  obstacles,  can  indeed  approach  m^,  the  complexity  of  the 
EDR  regions  is  roughly  linear  in  n,  (and  therefore  merely  quadratic  in  m).  This 
is  probably  due  to  the  structure  of  the  projection  regions.  Each  projection  region 
contsdns  free-space  edges  and  obstacle  edges.  Because  the  free-space  edges  of  any 
projection  region  are  in  at  most  two  orientations,  it  is  difficidt  to  get  crossings 
of  edges  in  free-space.  This  difficulty  is  exacerbated  by  the  fact  that  the  obstacle 
edges  in  two  intersecting  projection  regions  will  be  identical.  Thus  the  only  source 
of  quadratic  intersection  complexity  can  be  free-space  crossings  of  projection  edges. 
While  superlinear  intersection  complexity  seems  unlikely  for  projection  regions,  it 
remiiins  open  to  prove  a  linear  bound. 

6.4.  Critical  Slices;  An  Introduction 

While  upper  bounds  for  the  complexity  of  planning  in  a  single  slice  are  estab- 
lisheil,  the  complexity  of  planning  with  many  EDR  slices  is  less  well  understood. 
T wo  qiu’stions  remain: 


•  How  many  slices  are  necessary  for  EDR  planning? 

•  What  is  the  complexity  of  propagating  the  projection  regions  across  slices? 

The  key  to  answering  the  first  question  may  be  addressed  using  critical  slices. 

The  idea  is  as  follows.  Consider  the  gear  experiment,  where  gear  A  can  tramslate 
and  B  has  unknown  orientation.  Initially,  assume  that  the  orientation  of  B  is  fixed, 
so  it  cannot  rotate  when  pushed  by  A.  Let  a  denote  the  orientation  of  B.  Then 
consider  the  three-dimensional  backprojection  of  G  in  By  taking  x-y  slices  of 
the  backprojection  at  different  values  of  a,  it  is  clear  that  generically,  as  a  varies, 
the  topology  of  the  backprojection  remains  unchanged.  Similarly  for  the  forward 
projection  or  weah  backprojection;  The  topology  of  two  backprojection  slices  are 
the  same  if  no  edges  or  vertices  appear  or  disappear  at  a  values  between  them. 
At  singular  values  of  a,  however,  a  small  change  in  a  will  result  in  a  change  in 
the  topology  of  the  backprojection  slice.  Such  a  change  is  called  a  “catastrophe.” 
These  singular  values  are  called  critical  a,  and  the  generic  values  of  a  are  called  non- 
critical.  Two  critical  values  are  called  adjacent  if  there  is  no  critical  value  between 
them. 

The  idea  is  that  the  planning  algorithm  can  compute  a  backprojection  slice  at 
each  critical  value  of  o.  In  addition,  between  eaw:h  adjacent  pair  of  critical  values, 
the  algorithm  computes  a  slice  at  a  non-critical  a.  This  slice  of  the  backprojection 
at  that  value  is  representative  of  a  continutun  of  intermediate  non-critical  slices. 
Between  critical  slices,  in  addition,  it  is  clear  how  the  surfaces  of  the  backprojection 
change.  The  obstacle  vertices  of  the  backprojection,  for  example,  move  along  curved 
edges  that  are  algebraic  helicoids.  The  obstacle  edges  are  developable  algebraic 
smdaces.  The  equations  of  the  surfaces  are  found  in  [Brooks  and  Lozano-Perez]. 
The  equations  of  the  edges,  as  pareimeterized  by  orientation,  are  foimd  in  [Dl].  No 
additional  vertices  may  be  introduced  except  at  critical  values.  The  free-space  edges 
of  the  backprojection  remain  fixed  across  a  between  critical  values.  What  we  obtain 
is  a  complete  combinatorial  characterization  of  the  3D  backprojection  in  Q.  It  can 
be  used  to  derive  precise,  combinatorial  algorithms  for  decision  problems  about  the 
backprojection. 

For  example,  suppose  we  wish  to  decide  whether  a  start  region  R  is  contained  in 
the  3D  backprojection.  (That  is,  to  decide  whether  the  goal  is  guaranteed  reachable 
from  the  start  region).  This  problem  has  the  following  application.  By  deciding 
the  containment  question,  guaranteed  strategies  can  be  planned.  This  is  because 
backprojections  approximate  preimages.  Thus  by  deriving  upper  bounds  on  the 
containment  problem  in  the  backprojection,  we  obtain  bounds  for  the  planning  of 
guaranteed  strategies.  In  turn,  by  obtaining  bounds  on  the  guaranteed  planning 
problem,  we  can  gain  insight  into  the  complexity  of  EDR  planning. 

Suppose  R  has  the  form  U  x  J  for  U  a  polygon  in  the  plane.  Then  U  must 
be  tested  for  contairunent  in  each  critical  and  non-critical  slice  as  defined  above.  In 
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addition,  we  must  ensure  that  U  lies  inside  the  backprojection  as  the  boundaries 
of  the  backprojection  move  with  a.  Since  the  equations  of  these  sxirfaces  are  alge¬ 
braically  defined,  we  simply  test  them  for  intersection  with  the  boundary  edges  of 
U. 

The  next  question  is:  how  many  critical  values  of  a  are  there?  In  the  following 
lemma,  when  we  speak  of  edges  of  the  backprojection,  or  convex  configuration  space 
obstzw:le  (CO)  vertices,  we  mean  edges  of  the  backprojection  in  a  slice,  or  a  vertex 
which  is  convex  in  a  slice.  Of  course  these  edges  and  vertices  sweep  out  surfaces 
and  curves  (resp.)  as  a  changes. 

Lemma:  Lei  C  be  J  be  the  circle  S^.  Suppose  m  is  size  of  the  input  in  real- 
space  edges  so  that  n  =  O(m^)  is  the  number  of  generalized  configuration  space 
constraints.  Let  G  have  constant  size,  and  B$(G)  be  the  backprojection  of  G  in 
C  X  J  as  above.  Then  there  are  O(n^)  critical  values  of  a  £  J  for  Be(G). 

Proof:  We  enumerate  the  various  types  of  critical  values: 

A.  First,  an  a  value  is  (potentially)  critical  when  a  new  edge  or  vertex  is  intro¬ 
duced  into,  or  disappears  from,  the  union  of  the  configuration  sprw:e  obstru;les. 
This  can  introduce  a  topological  change  in  the  obstacle  boundary  of  the  back- 
projection.  If  A  and  B  are  convex,  then  as  a  varies,  there  are  potentially  m? 
topological  changes  in  the  configuration  space  obstacles.  These  generate  0{m?) 
critical  values  of  a,  which  we  call  oiitaclc-critical.  However,  when  A  and  B  are 
non-convex,  there  can  be  0(m®)  obstacle-criticad  values.  This  bound  arises  as 
the  number  of  critical  values  for  an  arrangement  of  surfaces  in  dimension 
d=  3. 

B.  In  addition,  an  a  value  can  be  critical  if  the  determination  of  sliding  vs.  sticking 
on  ^^n  edge  can  change  there.  A  change  in  sliding  can  result  in  the  introduction 
or  deletion  of  a  free-space  constraint,  and  hence  change  the  free-space  boundary 
of  the  backprojection.  This  occurs  when  an  edge  of  the  fidction  cone  on  some 
edge  becomes  parallel  to  an  edge  of  the  velocity  cone  of  control  imcertainty. 
Now,  as  a  configuration  space  edge  rotates  with  a,  its  friction  cone  rotates  with 
it.  Thus  as  a  changes,  a  friction  cone  edge  can  be  parallel  to  a  velocity  cone 
edge  at  most  4  times.  Hence  there  can  be  at  most  4n  values  of  q  at  which  thf 
sliding  determination  changes.  These  values  are  called  sliding-critical. 

C.  Next,  the  topology  of  a  slice  of  BgiG)  can  change  when  a  convex  vertex  of  a 
rotating  configuration  space  obstacle  edge  touches  a  free-space  edge  of  the  back- 
projection.  These  o-values  are  called  vertex-critical.  Now,  each  free-spare  edge 
of  a  backprojection  slice  is  anchored  at  a  convex  configuration-space  obsiarle 
(CO)  vertex.  Vertex-criticality  occurs  when  a  free-space  edge  of  a  backprojec¬ 
tion  slice  joins  two  CO  vertices  in  that  slice.  The  edge  then  lies  in  the  visibility 
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graph  of  the  generalized  configuration  space  obstacles  in  that  slice.  Now,  we 
can  obtain  a  bound  of  0(m®)  on  the  number  of  vertex-critical  values  as  fol¬ 
lows.  Introduce  an  additional  O(m^)  constraints,  each  anchored  at  a  convex 
CO  vertex  and  parallel  to  the  left  or  right  edge  of  the  velocity  cone.  These, 
together  with  the  O(m^)  obstacle  surface  constraints  form  an  arrangement  of 
O(m^)  surfaces  in  3  dimensions,  yielding  a  total  of  0(m®)  critical  values.  This 
boimd  may  be  improved  to  0(m*),  by  observing  that  each  vertex-critical  value 
is  generated  by  a  pair  of  convex  CO  vertices,  and  that  there  exist  O(m^)  such 
vertices. 

D.  Finally,  an  edge-critical  value  occurs  when  a  configuration  space  edge,  rotating 
with  a,  touches  a  free-space  backprojection  vertex.  Free-space  backprojection 
vertices  are  formed  by  the  intersection  of  two  free-space  edges  of  the  backpro¬ 
jection.  Each  free-space  edge  of  the  backprojection  is  anchored  at  a  convex  CO 
vertex.  The  number  of  edge-critical  values  is  0(m®),  because  each  is  generated 
by  a  CO  edge,  and  two  convex  CO  vertices  (one  per  free-space  backprojection 
edge). 

Finally,  we  observe  that  these  bounds  additive,  and  that  n  is  O(m^).  □ 

Comments'.  We  conjecture  that  the  bounds  on  edge-critical  values  (D)  can  be 
improved  to  0(m*).  One  approach  to  proving  the  improved  boimd  is  to  identify 
each  free-space  vertex  v  of  the  backprojection,  with  the  right  generating  CO  vertex. 
Follow  the  locus  of  u  as  a  varies.  It  remains  to  show  that  the  locus  is  piecewise- 
smooth,  and  touches  each  CO  edge  at  most  a  fixed  number  of  times. 

We  can  now  address  the  complexity  of  deciding  containment  in  the  backpro¬ 
jection.  In  this  discussion  we  address  only  the  combinatorial  complexity,  and  not 
the  algebraic  complexity,  of  the  decision  procedure.  Here  is  what  this  means.  We 
have  obtained  a  combinatorial  upper  bound  on  the  number  of  critical  values  of  a. 
Once  rotations  are  algebraically  parameterized,  these  critical  values  are,  in  fact,  all 
algebraic;  that  is,  in  general,  a  criticed  value  of  a  will  be  an  algebraic,  but  not  nec¬ 
essarily  rational  number.  However,  the  plane  sweep  algorithm  (which  is  discussed 
in  more  detail  in  chap.  VI)  operates  on  rational  numbers.  Hence  to  obtain  a  deci¬ 
sion  procedure  one  must  first  approximate  a  by  a  “nearby”  rational  number  q.  By 
“nearby”,  we  mean  that  the  approximation  must  be  known  to  be  sufficiently  close 
so  that  the  decision  procedure  will  give  the  correct  answer  for  containment.  As  the 
approximation  gets  closer,  the  rational  numbers  will  become  more  “larger”  (i.e.,  as 
a  quotient  of  two  integers,  the  integers  will  become  larger),  and  the  plane  sweep 
algorithm  will  run  correspondingly  slower.  To  choose  q  algorithmically,  one  might 
make  use  of  a  “gap”  theorem,  such  as  in  [Canny]. 
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Theorem:  Lei  U  be  a  polygon  of  constant  size,  C  be  J  be  S’*,  B${G)  be  the 
backprojection  of  G  in  C  x  J  as  above.  Suppose  G  jj  of  constant  size.  Then  there 
exists  an  algorithm  deciding  the  containment  of  R  =  U  x  J  in  Bff(G)  in  time 

0(n*  logn). 

Proof.  O(n^)  slices  of  the  backprojection  can  be  computed  in  time  0{n*  log  n).  Now, 
to  test  for  containment  of  U  in  the  3D  backprojection  region  between  two  adjacent 
critical  slices  will  take  time  0(n),  since  the  backprojection  has  size  0{n).  The  cost 
of  deciding  the  containment  of  U  between  successive  adjacent  pairs  of  n®  slices, 
each  of  size  n,  is  0(n*).  Since  the  time  for  computation  of  the  slices  dominates, 
this  yields  total  complexity  0(n*  logn).  □ 


Some  comments  are  in  order.  First,  our  algorithm  is  naive,  in  that  each  back- 
projection  shce  is  recomputed  from  scratch.  In  fact,  this  extra  work  is  tmnecessary. 
At  a  critical  value  of  or,  very  few  aspects  of  the  topology  of  the  backprojection  will 
change.  That  is,  typically,  only  one  or  two  edges  will  be  introduced  or  disappear 
at  any  critical  value.  We  can  make  this  notion  precise  as  follows.  If  a  is  a  generic 
singularity,  then  exactly  one  edge  or  vertex  will  appear  or  disappear  there.  Hence, 
for  example,  we  can  ensure  that  all  critical  values  are  generic  singularities  with 
probability  one  by  subjecting  the  input  to  small  rationed  perturbations. 

Suppose  that  a  backprojection  has  been  computed  in  a  critical  slice  at  q.  Then 
to  compute  a  backprojection  in  a  nearby  non-critical  slice  at  a  +  e,  we  merely  need 
to  update  the  portion  of  the  backprojection  boundary  that  was  critical  at  a.  This 
requires  only  constant  work:  only  one  edge  or  vertex  must  be  changed  to  derive  a 
backprojection  in  the  new  slice!  It  seems  reasonable  to  conjecture  that  this  tech¬ 
nique  would  yield  an  algorithm  of  complexity  0(n^  logo)  for  deciding  containment 
in  a  bau:kprojection. 

Finally,  it  appears  that  there  are  many  problems  in  which  the  number  of  cntical 
\'alues  fails  to  achieve  the  theoretically  possible  bound.  This  is  because  charac¬ 
teristically,  there  are  orientation  restrictions;  typically,  even  with  model  error.  B  is 
not  allowed  to  rotate  freely.  In  other  cases,  there  are  symmetries.  For  example,  in 
the  gear  case,  even  though  B  is  allowed  to  rotate  freely,  it  is  unnecessary  to  consider 
n’  shces  since  due  to  symmetry  the  configuration  spaces  “repeat"  pencxiically 

6.4.1  Comparison  with  Lower  Bounds 

From  a  theoretical  point  of  view,  this  n'sult  lias  the  follmvinst  uiteit'st  Con 
sider  the  one-step  cornpliant  motion  planiuiiK  protdeiu  in  3D  amidst  prts  is-  1 .  km -wn 
polyliedral  obstacles.  This  {>rohlem  may  lx- addressed  \,  )a  3D  Iwickpi ojfct  ions  in  1?’ 
CR  have  shown  t hat  df'cnlmg  containiiu'iif  in  such  a  3D  backjin  jci  rion  is  NT  hard. 


In  particular,  such  backprojections  can  have  an  exponential  number  of  faces.  How¬ 
ever,  in  the  previous  theorem  we  demonstrated  a  special  class  of  3D  backprojections 
that  have  only  0{n*)  faces,  along  with  an  efficient  algorithm  for  deciding  contain¬ 
ment.  This  special  class  of  backprojections  arises  in  the  presence  of  model  error. 
Specifically,  they  arise  when  C  is  3?^,  J  is  one-dimensional,  and  no  motion  is  per¬ 
mitted  across  J.  In  this  case,  the  non-holonomic  constraints  that  keep  the  robot 
within  one  slice  essentially  disallow  the  kind  of  fanning  out  and  branching  that 
^CR]  discovered  in  3?^.  Thus,  our  polynomial- time  algorithm  identifies  a  tractable 
subclass  of  the  3D  motion  planning  problem  with  uncertainty.  This  subclass  is  also 
interesting  in  that  it  arises  naturally  in  planning  with  model  uncertainty. 

6.4.2  Issues  in  the  Critical  Slice  Method 

The  critical  slice  method  represents  a  theoretical  algorithm.  It  has  not  been 
implemented  in  LIMITED.  It  was  described  here  to  give  some  characterization  for 
bounds  on  planning  with  model  error.  In  particular,  it  gives  a  precise,  combina¬ 
torial  description  for  the  3D  backprojection  in  x  S^,  and  an  exact  algorithm 
for  deciding  containment.  The  containment  algorithm  directly  addresses  the  ques¬ 
tion  of  planning  guaranteed  strategies,  since  a  backchaining  preimage  planner  can 
he  constructed  by  approximating  preimages  using  backprojections.  The  termina¬ 
tion  condition  for  such  a  planner  is  when  the  start  region  is  contained  within  a 
liackprojection. 

Most  important,  the  critical  slice  method  attempts  to  put  the  slice  techniques 
u.sed  in  LIMITED  on  a  firm  mathematical  footing."*  It  provides  a  principled  way — a 
sjiecific  method — for  choosing  which  slices  to  consider,  a  bound  on  how  many  slices 
are  required,  and  a  coi-servative  algorithm  for  deciding  containment. 

M  uch  work  remains  however; 

•  We  have  only  addressed  deciding  the  containment  problem  in  a  precise  com¬ 
binatorial  fashion.  Generalize  to  computing  set-differences  and  to  deciding 
their  (listinguishability — that  is,  deciding  G  vs.  H  distinguishability — using 
?he  rritical  slice  approach. 

•  J  IS  one  dimensional  in  our  discussion.  Generalize  the  critical  slice  method  to 
iimlf i-diinensional  model  error. 

•  T  hi."  analysis  addrf'si-o  the  complexity  of  verifying  an  EDR  strategy,  but  does 
niir  -peak  to  the  complexity  of  the  search.  W’hat  is  the  complexity  of  finding 
a  -rrateg;y  or  ilett'miininB:  that  none  exists"’  This  issue  will  be  attacked  in  a 
;ar»-r  -'''  Moii.  by  developing  a  combinatorial  description  of  the  non-directional 
oai'kp:  ojcc  r  a  >11 

‘  \  ,t.-  M  ,it  ..  .  h>i\e  fxH’ii  -lutiied  in  other  domairi.s.  See,  for  example,  [Lozano- 

1  .  /  s,  ;  u.ir'/  and  'l.tfc  1  rdmanii  . 


•  Derive  bounds  on  deciding  containment  after  relaxing  the  no-pushing  restric¬ 
tion  and  allowing  motion  across  J. 

Let  us  say  a  few  words  about  the  last  point.  Suppose  now  that  B  can  rotate 
passively  when  pushed.  Hence  motion  across  J  is  possible,  and  projection  regions 
must  be  propagated  across  slices.  For  example,  a  forward  projection  can  begin  in 
free-space  in  one  slice,  contact  an  obstacle  edge  generated  by  5,  rotate  across  J 
into  another  slice,  and  fly  off  the  edge  into  free-space  in  that  slice.  Hence  forward 
projections  mtist  be  propagated  across  slices.  This  process  was  described  above 
in  sec.  6.1.  The  obvious  question  is:  What  is  the  complexity  of  propagating  the 
projection  regions  across  slices?  The  complexity  of  one  step  of  the  propagation  is 
not  difficult  to  derive.  For  example,  consider  the  forward  projection.  There  are 
0(n)  obstacle  edges  in  the  forward  projection  in  a  planar  slice.  For  each  edge, 
a  constant  time  quasi-static  analysis  is  performed  to  determine  whether  pushing 
against  that  edge  can  cause  rotation  of  B,  that  is,  motion  across  J.  See  fig.  38. 
If  so  the  forward  projection  must  be  propagated  along  that  algebraic  surface  into 
an  adjacent  slice.  This  can  result  in  a  propagated  start  region  of  size  0(n)  in  the 
adjacent  slice.  This  start  region  is  used  to  compute  a  new  forward  projection  in  that 
planar  slice.  See  figs.  39  and  40.  This  propagated  forward  projection  must  then 
be  unioned  with  any  other  forward  projections  within  that  slice.  See  fig.  43,  which 
is  a  detail  of  fig.  42.  When  does  the  propagation  process  terminate?  A  correct 
termination  condition  is:  Terminate  propagation  when  any  propagated  start  region 
lies  within  an  existing  forward  projection. 

Now  while  the  complexity  of  each  of  these  steps  is  known,  it  is  not  clear  how 
long  it  takes  for  the  propagation  process  to  terminate.  In  particular,  results  of  [CR] 
suggest  that  3D  forward  projections  may  even  have  exponential  size.®  Experimental 
•■vidence — the  backprojector  of  [Erdmann] — concurs.  Furthermore,  when  propaga- 
M.)n  is  permitted,  more  slices  may  be  required.  For  example,  it  is  conceivable  that  a 
pur  h  within  the  forward  projection  may  break  contact  and  fly  off  into  a  slice  which 
i-  Oft  ween  the  chosen  critical  values.  In  other  words,  propagation  may  increase  the 
nn'oer  of  critical  values.  The  additional  critical  values  can  occior  as  follows.  The 
;if  -weep  algorithm  is  only  correct  when  the  velocity  cone  is  smaller  than  the 
• ,  III  r(,nf  on  any  edge  (see  chapter  VI).  Hence  we  will  assume  it  is  convex.  Then 
Oil  '  ■  .ui  bf  broken  when  the  irmer  product  of  an  extremal  vector  in  the  velocity 
•  Hi  ( iMf  ward-facing  edge  normal  is  positive.  Hence  the  zero-crossings  of  this 
ar<  poffiitiallv  critical  values;  there  are  0(n)  such  values.  While  this 
I  '  .rnplf-xity  of  computing  projections  when  pushing  can  cause  motion 
•  ■,  r<--  ;  uf lifi  "fudy. 

(  '  •■nUiii  I'lwcr  IioiiikI  for  the  size  of  the  forward  projection  in  3?^ 

.  '.1.  '  -  It  to  determine  the  applicability  of  their  proof  in 


This  completes  the  informal  discussion  of  the  one-step  EDR  planner  in  LiMITKI  i 
Later  in  the  thesis  we  will  discuss  the  <ietails  of  the  plane-sweep  algontlim  an<i  hoiA 
Limited  implements  the  EDR  theory  to  compute  multi-step  strategy's.  In  tie 
next  section,  we  will  discuss  a  number  of  theoretical  and  practical  issues  relating  t' 
the  construction  and  implementation  of  the  one-step  EDR  planning  algonthrns  ii 

Limited. 
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Fik  32  The  con  ft  gu  rat  ion  ipace  for  the  gear  example  (fig  2)  at  one  a-ilice 
It  =  Ol  of  The  goal  region  «  the  “vaileyi"  of  the  cipace  obatacle  The  atart 
region  la  the  diamond  to  the  lower  left,  for  figa  32  39,  B  la  not  allowed  to 
rotate,  lo  no  motion  arroaa  J  la  poaaible 


Fig.  33.  The  strong  bnckprojection  in  slice  a  =  0  of  the  goals  m  fig-  32,  auuming 
that  B  cannot  rotate.  In  all  these  experiments,  the  coefficient  of  friction  is  taken 
to  be  .25. 
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Fig.  40  Now  Msume  that  B  can  rotate  when  pushed  (for  figs.  40—48).  Here  we 
show  the  region  within  the  forward  projection  (fig.  35)  from  which  it  it  possible 
to  exert  positive  torque  on  B.  This  region  is  called  the  differential  forward 
projection  across  J  in  the  +<»  direction. 
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F  H  4J  In  th*  ntx\  figures,  B  is  permitted  to  rotate  when  pushed.  The  pro¬ 
tection  regions  are  computed  across  J  by  the  propagation  and  union  algorithm. 

tihow  four  sltces  of  generaJiied  configuration  space,  at  a  =  0®, 6“,  12®,  and 
LS'’  The  projections  take  into  account  possible  rotation  of  B  under  pushing. 
Here  the  weak  backprojections  across  slices  are  shown.  The  “spikes”  represent 
regu  ns  from  which  jamming  of  the  gears  must  occur. 
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Fig.  45.  Detail  of  the  forward  projection  for  a  =  12  .  Note  the  effect  of  propaga¬ 
tion  in  the  clockwise-moet  region  of  the  forward  projection.  This  region  can  only 
be  reached  when  rotated  to  from  neighboring  slice.  The  shaded  region  shows  the 
portion  of  the  forward  projection  which  has  been  propagated  by  pushing  from 
slice  o  =  18®. 


7.  Implementing  One-Step  EDR  Planning  in  Limited 


In  this  section,  we  discuss  a  number  of  theoretical  and  practical  issues  relating 
to  the  construction  and  implementation  of  the  one-step  EDR  planning  algorithms 
in  Limited.  This  discussion  is  the  sequel  to  the  informal  description  of  one-step 
EDR  planning  in  section  6.  While  these  issues  arise  in  implementing  an  EDR 
planner,  they  illustrate  several  interesting  theoretic^d  and  prax:tical  points.  They 
also  introduce  a  series  of  more  advanced  concepts  that  are  required  to  make  the 
r  '  ^  theory  rigorous. 


7.1.  The  Search  for  a  Strategy 


Some  comments  are  in  order  about  the  search  for  a  commanded  motion  in 
Limited.  First  of  all,  the  commanded  motion  generator  is  obviously  trivial.  The 
initial  commanded  motion  LIMITED  tries  is  obtained  by  subtracting  the  centroids 
of  the  start  and  goal  regions.  LIMITED  then  searches  outwards  on  either  side  of  this 
direction.  Obviously,  such  a  search  strategy  is  not  complete.  Later,  we  will  discuss 
precise,  combinatorial  algorithms  for  a  complete  search  strategy  using  critical  slices 
of  the  non-directional  backprojection. 

Second,  since  the  search  is  relatively  blind,  it  may  take  a  while  to  converge.  A 
better  technique  for  generating  commanded  motions  to  try  might  involve  using  a 
path-planner  with  no  uncertainty  to  suggest  an  initial  path.  However,  the  focus  of 
this  research  is  not  on  optimizing  the  search  algorithm  in  the  planner,  but  rather 
on  testing  the  geometric  characterization  of  EDR. 

It  is  often  possible  to  generate  planning  strategies  that  satisfy  the  EDR  axioms 
trivially  by  always  failing.  For  example,  consider  a  strategy  that  moves  directly  away 
from  the  goal.  While  such  a  motion  may  be  useful  in  multi-step  EDR  strategies 
(see  below),  we  wish  to  disallow  them  for  one-step  EDR.  This  is  done  as  follows; 
on  one-step  planning  LIMITED  discards  all  strategies  whose  forward  projection  does 
not  intersect  the  goal.  This  heuristic  ensures  that  the  strategy  hsis  at  least  some 
chance  of  succeeding. 

A  better  approach  might  be  to  consider  the  size  of  the  intersection  of  the  goal 
and  the  forward  projection.  Such  a  technique  might  be  used  to  generate  probabilisti¬ 
cally  optimal  strategies.  In  particular,  it  would  give  a  criterion  for  choosing  between 
two  EDR  strategies.  For  exzunple,  it  might  be  possible  to  place  some  probability 
distribution  over  the  forward  projection.  Then  the  intersection  could  be  integrated 
with  respect  to  this  distribution  to  determine  the  probability  of  reaching  the  goal. 
In  addition,  different  sized  velocity  cones  could  be  employed  to  generate  forward 
projections  of  varying  likelihood.  To  be  more  than  a  mathematical  abstraction,  such 
a  technique  requires  a  better  understanding  of  the  probabilistic  characteristics  of 


generalized  damper  control  than  are  currently  available;  it  is  a  promising  direction 
for  future  research. 

7.2.  Termination  using  Contact  and  the  Role  of  Time 

In  i>olyhedral  environments  with  a  bounding  box,  sticking  termination  is  suf¬ 
ficient  to  ensure  that  all  pure  translations  eventually  terminate  [Buckley],  Let  us 
assume  the  sufiiciency  of  sticking  termination  for  the  discussion  below. 

Recall  step  (4)  of  algorithm  lEDR  in  sec.  6.2: 

4.  Lei  dG  and  dH  denote  the  set  of  obstacle  edges  within  G  and  H ,  resp.  De¬ 
termine  whether  the  regions  dH  and  dG  are  distinguishable  using  sensors.  If 
so,  then  uj  yields  one-step  EDR  strategy  which  recognizably  terminates  on  an 
obstacle  edge  in  G  or  H .  The  termination  condition  is  contact  with  or  sticking 
on  a  surface  in  G  or  H. 

If  the  termination  condition  can  be  “contact”  as  computed  in  step  4  then  imless 
G  H  can  be  recognized  using  position  and  force  sensing  alone,  a  termination 
predicate  with  time-out  must  be  employed.  This  is  because  it  is  insufiicient  to 
terminate  on  first  contact;  we  must  first  know  that  the  contact  lies  within  G  U  if. 
Ensuring  that  this  will  be  the  case  requires  indexing  the  forward  projection  by  time. 
(See  [Erdmann]  for  a  discussion  of  time-indexed  forward  projections).  However  if 
the  contact  regions  of  G  and  H  are  distinguishable,  then  so  are  the  possible  sticking 
regions,  since  these  are  contained  in  the  contact  regions.  So  if  dG  and  dH  are 
distingxiishable  using  sensors,  then  so  are  pushj(G)  and  pushj(H').  Hence  Vg  yields 
a  one-step  EDR  strategy  which  recognizably  terminates  by  sticking  in  pushfl(G)  or 
pushg(H). 

Thus  if  the  distinguishability  condition  in  step  (4)  is  met,  then  the  motion  can 
be  terminated  either  based  on  contact,  once  enough  time  has  elapsed  to  ensure  that 
GU  H  has  been  entered,  or  using  sticking,  if  the  nm-time  executor  does  not  have  a 
clock.  In  fact,  this  is  something  of  a  relief,  because  time-indexed  forward-projections 
appear  hard  to  compute. 

If  sticking  is  not  sufficient  to  terminate  all  motions,  then  termination  predicates 
with  time-out  can  be  employed  to  ensure  that  all  motions  terminate  somewhere. 
This  is  discussed  further  in  sec.  8.3. 

7.3.  Employing  History  in  EDR  Planning 

The  forward  projection  was  introduced  by  [Erdmann]  to  formalize  the  role  of 
history  in  [LMT]  guaranteed  plans.  He  formalized  the  notion  that  by  knowing  where 


a  motion  began,  a  planner  can  obtain  constraint  on  where  the  motion  can  terminate. 
This  constraint  can  be  very  useful  both  in  planning  guaranteed  and  EDR  strategies. 
We  now  describe  two  techniques  where  LIMITED  uses  history  to  constrain  planning 
and  aid  distinguishability. 

Throughout  the  rest  of  section  7,  the  goal  G,  the  EDR  region  H  =  Hq  U  H^, 
and  the  forward  projection  F0{R)  axe  all  taken  to  lie  in  phsise  space.  That  is,  they 
are  position  and  velocity  sets. 

7.3.3  The  Role  of  History  in  Constructing  ff. 

In  general,  LIMITED  approximates  preimages  using  backprojections.  There 
are  a  few  interesting  points  about  computing  this  approximation.  Recall  that  H, 
denotes  sticking  regions  within  the  weak  minus  strong  preimage.  To  see  that  H, 
is  contained  within  F$(R),  we  must  use  the  fact  that  preimages  in  fact  depend  on 
the  start  region  R  (see  [LMT]),  and  that  weak  and  strong  preimages  are  contained 
within  Fg{R)  [Erdmann].  We  postpone  further  discussion  of  this  p>oint  until  sec. 
8.1. 

Now  let  Kf  denote  sticking  regions  within  the  weak  minus  strong  backprojection. 
Now  we  ask:  can  H,  intersect  the  strong  backprojection  of  G,  Rfl(G)?  The  answer  is 
no.  If  I  is  in  /f,,  then  sticking  is  possible  there.  Hence  x  cannot  be  in  Bg{G),  since 
motions  from  x  can  stick  there  instead  of  reaching  G.  Thus,  in  particular,  Hg  does 
not  intersect  the  set  difference  formed  by  the  strong  backprojection  minus  the  strong 
preimage.  Thus  H,  C  K,  since  backprojections  are  “upper  bounds”  on  preimages.® 
Thus  Kg  is  a  conservative  approximation  to  Hg.  (By  conservative,  we  mean  that 
while  the  approximation  may  be  larger  than  Hg,  an  algorithm  approximating  Hg 
by  Kg  will  generate  correct  plans).  However,  we  also  have  the  inclusion 

HgCF0{R)nKg, 

where  R  denotes  the  start  region.  This  inclusion  derives  from  the  fact  that,  given 
that  the  motion  must  start  in  R,  a  strong  preimage  of  G  is  contained  within  the 
intersection  of  the  forward  projection  F0{R)  and  the  strong  backprojection  B0{G). 
A  similar  containment  holds  for  the  weak  preimage  with  respect  to  the  forward 
projection  and  the  weak  backprojection  WgiG).  Hence,  to  construct  an  upper 
bound  on  H,,  we  are  simply  required  to  compute  all  sticking  regions  within 

F9(R)n(W«(G)-R,(G)). 

The  advantage  of  using  this  tighter  approximation  is  that  Hg  must  be  distin¬ 
guishable  from  G  in  order  for  6  to  yield  an  EDR  strategy.  Tighter  approxima- 

®By  “upper  bound”  we  mean  that  the  backprojection  (under  9)  contains  the  preimage 
(under  9),  for  any  goal. 


tions  heviristically  result  in  a  greater  likelihood  of  distinguishability.  Furthermore, 
the  approximation  is  fully  algorithmic,  since  projections  and  set  differences  can  be 
computed  by  the  exact  plane-sweep  algorithm  described  in  sec.  6  . 

The  dependence  of  the  preimage  on  R  was  noted  first  in  [LMT]  and  later 
developed  by  [Erdmann],  who  also  determined  the  inclusion  of  the  preimage  within 
the  intersection  of  the  forward  and  backprojections.  This  dependence  is  discussed 
in  some  detail  in  chapter  III. 

7.3.4  The  Role  of  History  in  Distinguishing  H  from  G 

There  is  £inother  case  where  intersection  with  the  forward  projection  yields  an 
advantage  in  distinguishability.  In  the  development  of  EDR  theory,  we  foimd  that  in 
an  EDR  strategy,  it  was  necessary  that  H  and  G  be  distinguishable.  This  condition 
is  supplanted  by  a  tighter  one  in  LIMITED,  requiring  only  that  H  and 

Fe(R)  n  G 

be  distinguishable.  The  justification  is  that  it  is  imnecessary  to  be  able  to  distinguish 
between  arbitrary  points  of  G  and  H.  Instead,  it  suffices  merely  to  distingmsh 
between  the  reachable  points.  Again,  this  technique  shrinks  the  size  of  the  goal 
regions  that  must  be  distinguishable  from  ff,  which  heuristically  makes  it  easier 
for  a  strategy  to  meet  the  EDR  criteria.  Note  that  H  does  not  need  to  be  shrunk 
further,  becatise  it  already  lies  within  the  forward  projection. 

7.4.  A  Priori  Push-Forwards  Based  on  Sticking 

The  push-forward  operator  push^(-)  is  a  restricted  implementation  of  the  gen¬ 
eral  push-forward  described  in  sections  4  and  10.  It  is  called  the  a  priori  push- 
forward  based  on  sticking.  The  difference  is  ^ls  follows.  The  theoretical  general 
push-forward  “decides”  where  the  motion  should  terminate  so  that  the  motion 
strategy  will  terminate  recognizably,  the  a  priori  push-forward  computes  where 
the  motion  will  terminate  given  that  a  priori  the  termination  condition  is  sticking. 

7.5.  Sticking  as  a  Robust  Subtheory  of  EDR 

In  the  abstract  EDR  theory,  one  envisions  the  run-time  termination  predicate 
performing  whatever  computations  are  necessary  to  terminate  a  motion  recogniz¬ 
ably  in  G  or  H.  That  is,  in  principle,  the  planner  decides  what  termination  con¬ 
ditions  are  appropriate  for  a  successful  EDR  strategy,  and  encodes  them  into  the 
motion  strategy.  Of  course,  it  is  also  the  responsibility  of  the  planner  to  verify 
that  this  encoding  will  always  result  in  a  distinguishable  termination.  In  short,  the 


abstract  EDR  theory  can  employ  the  full  power  of  the  [LMT]  preimage  framework 
to  generate  motion  strategies. 

However,  LIMITED  employs  only  certain  restricted  termination  conditions,  as 
we  saw  above.  In  paxticular,  sticking  is  used  in  most  experiments.  This  restricts  the 
class  of  strategies  LIMITED  can  generate.  The  restriction  requires  some  justification, 
jind  that  is  the  purpose  of  this  section. 

First,  recall  that  in  polyhedral  environments  with  a  bounding  box,  sticking 
termination  is  sufficient  to  ensure  that  all  pure  translations  eventually  terminate 
[Buckley].  In  general,  in  this  thesis  we  have  made  the  heuristic  assinnption  that 
motions  can  eventually  be  terminated  via  sticking.  Failing  this,  we  also  entertain 
the  weaker  assumption  that  if  sticking  is  insufficient,  then  time  can  be  employed  to 
wait  until  G  \J  H  has  been  achieved  before  termination;  see  sec.  8.3. 

To  analyze  the  structure  of  sticking  termination,  let  us  introduce  the  following 
notation.  If  the  robot  recognizably  achieves  G\J  H ,  this  means  that  the  run-time 
executor  can  determine  that  G  or  H  has  been  achieved,  but  cannot  necessarily  tell 
which  oiG  OT  H  has  been  entered.  If  the  robot  recognizably  achieves  {G,H  },  then 
it  can  further  distinguish  which  of  G  or  if  it  has  reached.  G  U  if  is  called  the  union 
while  the  set  notation  is  called  the  distinguishable  union. 

Throughout  this  section  we  assume  without  loss  of  generality  that  the  goal  G 
is  contained  within  the  forward  projection  (see  sec.  7.3  for  justification).  If  this  is 
not  the  case,  then  intersect  them  to  obtain  a  new  goal. 

Limited  tries  to  decompose  this  problem — of  ensuring  that  all  trajectories 
terminate  recognizably  in  {  G,  if  } — into  two  subproblems.  The  first  is  to  ensure 
that  the  motion  in  fact  terminates  in  G  U  if .  That  is,  the  problem  is  to  determine 
that  at  least  one  of  G  or  if  has  been  achieved,  although  the  robot  may  not  know 
which.  The  second  problem  is  to  distinguish  between  G  and  if,  once  G  U  if  hais 
been  achieved. 

Note  that  the  first  problem  requires  distinguishing  between  G  U  if  and  its 
complement.  Here  is  the  key  point: 

•  The  construction  of  if  guarantees  tautologously  that  with  sticking  termination, 
GUif  will  be  recognizably  achieved  when  the  motion  terminates.  That  is,  with 
sticking  termination,  no  motion  can  terminate  outside  of  G  U  if . 

This  resolves  the  first  subproblem.  Thus 

•  With  sticking  termination,  all  candidate  one-step  EDR  strategies  eventually 
terminate  recognizably  m  G  U  if  (but  not  necessarily  in  {G,  H  }).  Of  these,  all 
valid  EDR  strategies  can  distinguish  between  G  and  if  after  termination,  and 
hence  recognizably  terminate  in  {G,H}. 

The  second  subproblem  is  how  to  distinguish  between  G  and  H  once  G  U  if 
has  been  achieved.  In  developing  the  [LMT]  framework  for  planning  guaranteed 
strategies,  [Erdmann]  developed  an  elegant  formalization  of  the  question,  “Using 


sensors  and  history,  when  can  the  termination  predicate  decide  that  a  motion  has 
recognizably  entered  a  goal  G/j?”  The  answer  weis  as  follows.  Let  R  be  the  start 
region.  The  forward  projection,  FeiR)  captures  the  notion  of  history:  it  is  all 
positions  and  velocities  that  can  be  reached  given  that  the  motion  stairted  in  R.  At 
a  particular  instant  t  in  time,  let  and  be  the  sets  of  possible  positions 

and  velocities.  These  are  the  sensing  uncertainty  balls  about  a  sensed  position  and 
velocity  in  phase  space  at  time  t.  Thus  sensing  provides  the  information  that  the 
actual  position  and  velocity  must  lie  within  the  set  Btp{t)  x  Bt.„{i).  The  forward 
projection  further  constrains  the  actual  position  and  velocity  to  lie  within  Fe{R). 
Thus  the  termination  predicate  can  terminate  the  motion  as  having  recognizably 
reached  G  when 

FeiR)  n  {B,p{t)  X  C  G^.  (*) 

Now,  when  is  it  the  case  that  the  termination  predicate  can  distinguish  which 
of  G  or  /f  has  been  reached?  Exactly  when  (*)  is  true  for  G^  in  {  G,  if  }.  However, 
in  our  case,  sticking  termination  guarantees  that  the  actual  position  and  velocity 
lie  within  G  U  if .  Furthermore,  G  U  ff  is  a  subset  of  the  forward  projection,  and 
G  and  H  are  disjoint  by  construction.  The  forward  projection  provided  no  further 
constraint  in  distinguishing  between  G  and  H.  Thus  history  plays  no  role  in  the 
run-time  distinguishing  actions  of  the  robot  executive;  history  has  been  pre-encoded 
into  the  structure  of  H.  Hence,  we  can  predict  that  the  run-time  executor  can 
distinguish  which  of  G  or  H  has  been  achieved  when  the  planner  can  predict  that  G 
and  H  are  distinguishable  using  sensors  alone.  A  procedure — albeit  not  completely 
general — for  deciding  this  question  was  described  in  sec.  6.2. 

7.5.5  Generalizations 

There  are  several  possible  generalizations  of  these  termination  techniques.  First, 
it  may  be  possible  for  the  run-time  executor  to  use  time  to  ensure  that  the  motion 
terminates  in  G  U  H.  That  is,  forward  projections  may,  in  principle,  be  indexed  by 
time.  Hence  in  (♦),  Fj(i2)  is  replaced  by  Fg{R,t),  which  is  typically  much  smaller. 
Fg{R,t)  denotes  the  set  of  positions  and  velocities  that  are  possibly  achievable  at 
elapsed  time  t,  under  motion  9,  given  that  the  motion  started  in  R.  The  termination 
predicate  in  this  case  monitors  a  clock,  in  addition  to  position  and  velocity  sensors. 
However,  in  this  case,  history  (by  which  we  meain  Fd{R,t))  could  be  employed 
to  distinguish  G  from  H,  even  though  the  motion  had  terminated  recognizably  in 
GuH.  The  reason  for  this  is  that  the  time-indexed  forward  projection  has  not  been 
pre-encoded  into  the  structure  of  H .  That  is,  H  was  constructed  using  the  timeless 
forward  projection,  which  the  union  of  all  time-indexed  forward  projections.  Hence, 
we  can  summarize  these  observations  as  follows: 


•  If  a  termination  predicate  without  time  uses  sticking  to  terminate  the  motion, 
then  distinguishing  G  from  H  is  a  history-free  decision.  However,  for  a  termi¬ 
nation  predicate  with  a  sense  of  time,  the  decision  is  not  history-free. 

Thus  sticking  subtheory  does  not  preclude  more  general  termination  techniques 
based  on  position,  force,  and  time  sensing.  However,  two  computational  issues  be¬ 
come  more  difficult.  First,  sticking  termination  is  a  robust  method  for  ensuring 
termination  in  G  U  2?.  With  time  termination,  or  more  general  position/force  ter¬ 
mination  criteria,  it  is  more  difficult  to  ensure  termination  in  G  U  H — although 
admittedly  these  criteria  are  more  powerful.  Second,  after  sticking  termination,  de¬ 
ciding  between  G  and  H  is  history-free.  With  more  general  termination  predicates, 
history  can  provide  extra  constraint  in  distinguishing  between  G  and  H. 

Finally,  note  that  [Buckley]  recognized  the  value  of  sticking  termination  when 
implementing  an  [LMT]  planner  for  guaranteed  strategies  in  3?^.  His  planner  used 
sticking  termination.  In  particular,  he  provided  certain  criteria  for  guaranteeing 
that  a  strategy  eventually  terminates  in  sticking.  Buckley’s  criteria  amount  to 
ensuring  that  the  environment  is  finite  polyhedral,  within  a  bounding  polyhedral 
box. 

7.5.6  Forward  vs.  Backward  Chaining 

One  obvious  disadvantage  of  sticking  termination  is  that  it  is  not  complete.  For 
example,  a  planner  employing  sticking  termination  exclusively  will  not  be  able  to 
find  strategies  that  require  “stopping  in  mid-air”,  even  when  such  strategies  would 
be  feasible  given  the  position  sensing  accuracy  of  the  robot.  Sticking  termination 
requires  aU  strategies  to  “rtm  aground”,  that  is,  to  be  in  contact  (and  in  fact, 
sticking)  at  termination  time. 

With  more  general  position/force/time  termination  criteria,  the  requirement 
that  motions  must  terminate  in  contact  is  relaxed.  However,  a  forward-chaining 
planner  (such  as  LIMITED)  is  still  left  with  the  problem  of  deciding  where  a  motion 
should  terminate  in  a  multi-step  strategy.  That  is,  the  decision  problem  involves 
existential  quantification  not  only  over  the  commanded  directions,  but  also  over 
all  subsets  of  the  forward  projection  corresponding  to  possible  push-forwards.  Put 
simply,  a  forward-chaining  planner  must  not  only  guess  the  direction  to  command  a 
motion,  but  must  also  guess  where  it  terminates,  before  chaining  ahead  to  the  next 
motion.  While  the  space  of  commanded  motions  may  be  realistically  quantized  and 
searched,  the  space  of  push-forwards  may  not  be  searched  in  this  manner. 

While  Li.VIITED  is  a  forward-chaining  planner,  the  problem  of  existential  quan¬ 
tification  over  the  push-forward  is  finessed  by  restricting  LIMITED  to  a  few  very 
simple  termination  conditions  (there  are  only  three;  see  sec.  6.2),  one  of  wliich  is 
sticking.  Given  these  termination  types,  it  is  possible  to  generate  the  corresponding 


a  priori  push-forwards,  and  test  them  to  see  whether  they  yield  an  EDR  strategy. 
For  example,  the  push-forward  for  contact  termination  is  simply  the  obstacle  edges 
in  the  forward  projection.  The  push-forward  for  sticking  termination  is  the  a  priori 
push-forward  based  on  sticking,  which  was  discussed  above. 

More  generally,  it  may  be  possible  to  define  a  parameterized  family  of  termina¬ 
tion  predicates,  each  with  an  associated  a  priori  push-forward.  Each  push-forward 
could  then  be  tested  for  distinguishability.  For  example,  consider  the  class  of  ter¬ 
mination  conditions 

{  “Terminate  after  t  seconds.”  |  <  >  0  } 

An  associated  family  of  push-forwards  might  be  the  time-indexed  forward  projec¬ 
tions 

{F0iR,t)\t>O}. 

However,  the  existential  quantification  over  the  push-forward  in  the  decision 
problem  for  EDR  planning  is,  in  fact,  an  artifact  of  forward-chaining.  We  can  see 
this  by  comparing  and  contrasting  backchaining  vs.  forward-chaining  in  preim¬ 
age  planners  for  guaranteed  strategies.  In  a  backward-chaining  planner,  this  extra 
computation  is  eliminated.  The  difference  is  as  follows.  Consider  how  a  guaranteed- 
strategy  preimage  planner  would  construct  a  motion  strategy  , . . , ,  to  achieve 
a  goal  G.  01  is  the  first  motion  in  the  plan,  d„  is  the  last.  Consider  the  difference  in 
how  a  forward-chaining  planner  and  a  backchaining  planner  would  compute  step® 
0i  and  Bi^i'. 

•  A  forward-chaining  planner  must  calculate  where  motion  9i  will  terminate, 
since  this  termination  region  is  the  start  region  for  the  next  motion,  Oi+i- 
Since  this  calculation  involves  some  choice,  it  amounts  to  a  formulation  of 
the  decision  problem  with  existential  quantification  over  the  push-forward  of 
motion  0i.  In  a  b2u:k-chaining  planner,  where  the  motion  must  terminate  has 
already  been  computed:  it  is  the  next  preimage  with  respect  to  ^j+i,  naunely 
P9,^APb,^A---{P9.(G))  ■■■)). 

Thus  we  have  seen  why  a  back-chaining  planner  can  (in  principle)  be  complete 
for  guaranteed  strategies,  while  a  forward-chaining  planner  cannot,  unless  it  guesses 
push-forwards. 

This  suggests  the  following  approach  to  EDR  planning: 

•  Use  a  back-chaining  planner  to  find  a  guaranteed  strategy  for  pari  of  the  start 
region.  Then  extend  it  to  an  EDR  strategy  using  forward -chaining  verification. 
This  appears  to  be  a  reasonable  heuristic  approach.  However,  for  EDR  plan¬ 
ning,  it  is  still  merely  a  halfway  measure.  While  it  removes  from  the  EDR  planner’s 
responsibility  the  decision  of  where  to  terminate  a  motion  within  a  subgoal,  the 
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problem  remains  of  deciding  where  within  the  EDR  region  H  a  motion  should  ter¬ 
minate.  This  is  one  of  the  key  theoretical  questions  in  EDR;  it  is  addressed  at 
some  length  later.  The  computational  solution  seems  to  involve  quantifying  over 
push-forwards  even  when  a  combination  backward-  and  forward-chaining  planner 
is  envisioned.  LIMITED  uses  only  forward-chaining  for  this  reason.  However,  the 
combination  back-  and  forward-chaining  approach  deserves  more  exploration.  In 
particular,  the  backchaining  first  stage  could  be  used  to  suggest  and  guide  the 
search  for  good  candidate  EDR  strategies.  Randy  Brost  has  reported’^  a  backchain¬ 
ing  planning  algorithm  which  can  generate  multi-step  plans  in  which  each  motion 
is  a  one-step  EDR  strategy. 


^[Personal  Communication].  See  also  Brost’s  forthcoming  Ph.D.  thesis. 


8.  The  Preimage  Structure  of  EDR  Regions 


Our  characterization  of  the  EDR  regions — and  of  EDR  strategies  in  general — 
hzis  been  somewhat  informal  up  to  now.  This  is  because  we  have  not  employed  the 
full  power  of  the  preimage  framework  [LMT]  in  developing  the  model.  In  particular, 
we  have  not  yet  tied  together  the  role  of  history  and  the  definition  of  the  EDR 
region  H.  This  section  remedies  this  deficiency.  Recall  our  characterization  of  H 
a.s  Hq  U  Hf.  Our  definition  of  phase-space,  and  of  the  sticking  region  Ha  must  be 
be  made  more  precise.  That  is  the  second  goal  of  this  section:  to  link  the  preimage 
concepts  for  representing  strategies,  with  the  phase-space  concepts  for  representing 
error  detection  in  generalized  configuration  space. 

First,  we  briefly  introduce  some  preimage  notation.  A  formal  review  of  [LMT,E] 
preimages  can  be  found  in  an  appendix.  A  key  notion  deals  with  recognizable  termi¬ 
nation  in  a  collection  of  goals.  We  can  illustrate  this  notion  using  a  familiar  example: 
for  a  goal  G  and  am  EDR  region  ff,  {  G,  }  is  a  natural  collection  of  goals  to  con¬ 
sider.  We  know  that  an  EDR  strategy  recognizably  terminates  in  some  member 
of  the  collection.  Below,  we  give  a  formal  notation  for  this  concept.  However,  the 
question  of  distinguishable  collections  of  goals  also  arises  in  planning  guaranteed 
strategies,  as  we  shall  see. 

8.1.  On  the  Recognizability  of  EDR  regions 

In  section  7,  we  elaborated  on  the  role  of  history,  time,  position-sensing,  and 
force-sensing  in  motion  termination  and  in  distinguishing  G  from  H.  The  role  of 
history  must  be  formalized  further  using  the  preimage  framework.  To  this  end,  we 
now  formalize  the  distinguishability  requirements  for  G  and  H  using  preimages. 

In  section  2.1  it  was  observed  that  if  the  termination  predicate  can  distinguish 
between  the  goal  G  and  the  EDR  region  ff,  then  H  is  a,  good  EDR  region  and  an 
EDR  strategy  was  in  hand.  Formally,  we  write  this  recognizability  constraint  as* 

Pe,R{{G,H})  =  R.  (3) 

We  say  that  the  preimage  (3)  is  taken  with  respect  to  R.  (3)  means  that  the 
(strong)  preimage  of  the  set  of  goals  {G,H  },  with  respect  to  commanded  velocity 
Vg,  is  all  of  R.  When  we  have  a  set  of  goals,  the  termination  predicate  must  return 
which  goal  {G  or  H)  has  been  achieved.  This  is  different  from  Pg,R{G  U  H),  which 
means  the  termination  predicate  will  halt  saying  “we’ve  terminated  in  G  or  ,  but  I 
don’t  know  which.”  The  region  R  appears  on  both  sides  of  (3)  because  the  preimage 
depends  on  knowing  where  the  motion  started.  This  is  a  subtle  point,  see  [LMT,E]. 

*We  view  Pg  R  as  a  map.  In  the  informal  development  we  denoted  the  image  of  this 
preimage  map  by  P. 


Thiis  solving  preimage  equations  like  (3)  for  R  is  like  finding  the  fixed  point  of  a 
recursive  equation.  Here,  however,  we  know  i2,  H,  and  G,  so  (3)  is  a  constraint 
which  must  be  true,  rather  than  an  equation  to  solve.  Presumably  (3)  is  easier  to 
check  than  to  solve  for  R\  see  [LMT,E]. 

With  this  imderstood,  we  can  now  characterize  P  and  R'  precisely  (see  fig.  26). 
This  requires  specifying  the  start  regions  : 


R'  =  PeMG) 

P  =  P9,Ft(R){G). 

P  is  anzdogously  defined  by  adding  “hats”  to  the  P’s  in  (5). 


8.1.1  The  Most  Genered  Preimage  Equation 

We  now  introduce  the  most  general  form  of  the  preimage  equation.  Suppose 
{Gjs}  denotes  a  collection  of  goals,  and  {  Pq  }  is  a  collection  of  start  regions.  Recall 
9  denotes  the  direction  of  the  commanded  motion.  Most  generally,  the  preimage 
equation  is 


})={«.}• 

This  says  that  if  the  run-time  executor  knows  that  the  robot  is  in  some  particular  but 
arbitrary  start  region  R  in  the  collection  {  Ra  },  then  if  velocity  Vg  is  commanded, 
then  the  termination  predicate  is  guaranteed  to  achieve  some  goal  G  in  {  Gp  },  and, 
furthermore,  it  can  recognize  which  goal  has  been  achieved. 

In  chapter  III,  a  detailed  example  using  preimages  is  worked  out.  There,  we 
solve  a  particular  motion  planning  problem — grasp  centering —  with  model  error 
by  solving  the  preimage  equations.  This  example  provides  an  illustration  of  plan¬ 
ning  using  preimages.  Both  guaranteed  and  EDR  strategies  for  this  problem  are 
developed. 

8.2.  The  Structure  of  Goals  in  Phase  Space 


In  this  section,  we  examine  the  structure  of  phase  space  goals  in  some  detail, 
using  the  general  form  of  the  preimage  equations  described  above. 

A  goal  in  phase  space  is  a  region  in  Position-space  x  Velocity-space.  A  phase 
space  goal  is  attained  when  the  actual  position  and  velocity  can  be  guaranteed  to 
lie  in  the  region.  We  have  actually  been  using  pheise  space  goals  all  along,  since  the 


*■  ^  ^ 


•I  ^ 

^(f-n 


PhiMSfaci 


PocMMSfaci 


Fig.  49.  Position-space  is  one  dimensional.  Therefore  phase  space,  which  is 
Position-space  x  Velocity-space,  is  2-dimensional.  The  velocity  “axis”  is  shown 
vertically.  ir  projects  a  position  and  velocity  to  the  position.  We  lift  the  strong 
preimage  P  to  a  cylinder  jr~^(P),  We  also  obtain  the  cylinders  over  the  weak 
but  not  strong  preimage  P  —  P,  and  over  the  forward  projection  outside  the 
weak  preimage,  F  —  P. 


velocity  sensors  are  used  to  recognize  goals.  The  introduction  of  arbitrary  phase- 
space  goals  is  problematic,  see  [Erdmann].  Here  the  goals  are  sufficiently  simple 
that  these  dangers  are  avoided. 

We  begin  with  the  simpler  example.  In  fig.  26  we  proposed  a  partition  of  the 
forward  projection  F  o{  R  into  three  regions: 


Strong  preimage,  P 
Weak  but  not  strong  preimage,  P  —  P 
Forward  projection  outside  the  weak  preimage,  F  —  P. 


Here,  the  partition  was  “good”  for  the  purposes  of  EDR  for  all  velocities,  and 
we  could  let  H  be  the  forward  projection  outside  the  weak  preimage.  We  can 
extend  this  partition  into  phase  space  as  shown  in  fig.  49.  There  is  a  natural 
projection  tt  of  Position-space  x  Velocity-space  onto  Position-space  which  sends  a 
pair  (x,  velocity-at-x)  to  its  position  x.  Given  a  region  U  in  position  space,  we  can 
lift  it  to  phase  space  to  obtain  the  cylinder  of  all  velocities  over  U .  A  point 

in  'K~^{U)  is  (x,  v)  where  x  is  in  U,  and  v  is  any  velocity  at  x.® 

We  lift  the  partition  by  applying  the  inverse  projection  map  to  obtain  a  parti¬ 
tion  of  phase  space: 

Cylinder  over  strong  preimage, 

Cylinder  over  weak  but  not  strong  preimage,  —  P) 

Cylinder  over  forward  projection  outside  the  weak  preimage,  •k~^{F  —  P). 

See  fig.  49.  Now,  the  cylinder  over  G  and  the  cylinder  over  F  —  P  are  the  new  goals 
in  phase  space.  The  latter  cylinder  is  the  phase  space  EDR  region  for  fig.  26.  Both 
are  simply  cylinders:  all  velocities  are  legal. 

Now  we  must  deal  with  the  tricky  sticking  region  H,  in  fig.  27.  We  begin  by 
lifting  the  partition  to  phase  space  again  {see  fig.  50).  Next,  we  “mark  off”  regions 
in  the  lifted  partition  to  form  a  phase  spa.ce  EDR  region,  which  we  denote  H.  The 
entire  cylinder  over  F  —  P  is  clearly  in  since  its  projection  (under  t)  is  outside 
the  weak  preimage.  But  the  cylinder  over  If,  is  not  entirely  within  ff:  only  sticking 
velocities  over  If,  are. 

Formally,  ff,  is  the  set  of  all  points  x  in  the  weak  but  not  strong  preimage, 
such  that  sticking  can  occur  at  x.  We  wish  to  distinguish  the  sticking  velocities  in 
Pf.  Under  generalized  damper  dynamics,  these  are  essentially  the  zero  velocities. 

Le'"  Z{Ht)  denote  the  zero  velocities  over  H,,  that  is,  the  set  of  pairs  (x,0)  for  x 
in  Hs-  This  set  is  in  phase  space. Then  we  see  that  Z{Ha)  is  also  in  the  phase 
space  EDR  region  H.  Thus  H  is  the  union  of  the  sticking  velocities  over  Ha,  and 
all  velocities  over  the  forward  projection  outside  the  weak  preimage: 

H  =  ZiHa)Dw-^iF~P).  (6) 

®The  cylinders  may  then  be  intersected  with  the  forward  projection  of  R  (in  phase  space) 
to  obtain  more  constraint.  This  may  be  done  by  first  restricting  the  domain  of  n  to  the 
forward  projection. 

^°The  weak  and  strong  preimage,  and  the  forward  projection  are  drawn  Venn-diagrammatically 
in  one-dimension. 

We  could  also  let  Z(Ha)  be  the  set  of  velocities  over  Hs  which  are  smaller  than  some 
threshold. 


Fig.  50.  Compare  fig.  49.  We  have  indicated  the  sticking  region  H$  in  the  weak 
preimage.  The  zero- velocities  Z{H»)  over  H,  are  in  the  cylinder  over  H«.  The 
EDR  region  H  is  the  union  of  Z{Ht)  and  the  shaded  cylinders  over  the  forward 
projection  outside  the  weak  preimage  F  —  P. 


To  use  /I  as  an  EDR  region,  we  must  now  ensvure  that  H  and  the  cylinder  over 
G  are  distinguishable  goals.  This  amoimts  to  allowing  gocds  in  phase  space — that 
is,  allowing  the  preimage  operator  to  take  simple  phase  space  goals  as  arguments, 
and  rewriting  (3)  as 


=  (3a) 

The  impact  of  (3a)  is  discussed  in  more  detail  in  an  appendix.  One  point  is 
worthy  of  comment.  If  the  strong  preimage  is  known,  the  definition  of  (phase  spaw:e) 
EDR  regions  is  constructive  up  to  reachability.  By  this  we  mean  that  when  backpro- 
jections,  set  intersections  and  differences,  and  friction  cones  can  be  computed,  then 
so  can  H.  With  H  is  hand,  we  add  the  recognizability  constraint  (3a)  to  obtain  an 
EDR  strategy. 


8.3.  More  on  Weak  Preimages 


Armed  both  with  the  general  form  of  the  preimage  equation,  and  with  our 
new  grasp  of  the  structure  of  phase-space  goals,  we  now  examine  the  structure  of 
the  “weak  but  not  strong  preimage,”  P  —  P  in  more  detail.  It  suggests  a  number 
of  implementation  issues.  Consider  figs.  27,28  once  more.  Suppose  we  have  a 
trajectory  originating  in  P,  subject  to  the  control  uncertainty  shown.  We  do  not 
wish  to  terminate  the  motion  while  it  remains  in  the  weak  preimage,  since  fortuitous 
sensing  and  control  events  could  still  force  recognizable  termination  in  G.  However, 
we  can  terminate  the  motion  as  soon  as  we  recognize  egress  from  the  weak  preimage. 
This  is  why  the  forward  projection  outside  the  weak  preimage  is  contained  in  the 
EDR  region. 

As  we  have  seen,  however,  it  is  possible  for  a  trajectory  to  remain  within  the 
weak  but  not  strong  preimage  forever.  For  exzunple,  it  can  stick  in  H,  forever.  To 
handle  this  case,  we  introduced  phase  space  EDR  goals. 

There  are  other  conditions  under  which  a  trajectory  could  stay  in  P  — P  forever: 
(a)  if  the  environment  is  infinite,  or  P  — P  is  unbounded,  (b)  The  trajectory  “loops” 
in  P  —  P  forever,  (a)  and  (b)  are  qualitatively  different  from  the  case  of  sticking 
forever  in  Hg,  because  they  require  motion  for  infinitely  long.  In  practice  this  may 
be  handled  by  terminating  the  motion  in  P  —  P  after  a  certain  elapsed  time.  This  is 
called  “constructing  termination  predicates  which  time-out.”  In  fact,  this  “solution” 
works  for  sticking  in  Hg  also. 

An  alternative  is  to  extend  our  earlier  zero-velocity  analysis  to  all  of  P  —  P. 
That  is,  we  terminate  the  motion  in  the  weak  but  not  strong  preimage  when  the 
actual  velocity  is  (close  to)  zero.  Formally  this  rewrites  (6)  as 

P  =  Z(P-P)U7r-*(P-P).  (6a) 

Both  this  and  our  formal  handling  of  phase  space  goals  for  Hg  (6)  are  subject  to  the 
“Rolles  theorem  bug.”  That  is,  a  trajectory  which  “reverses  direction”  will  have 
zero  velocity  at  some  point.  Hence  by  (6,  6a)  it  will  be  judged  to  have  stuck.  This 
is  undesirable.  In  practice  this  can  be  fixed  by  again  requiring  the  trajectory  to 
stick  for  some  elapsed  time.  Time-out  termination  predicates  have  the  following 
practical  justification.  We  imagine  some  low-level  control  mechanism  which  detects 
sticking,  auid  after  a  certain  time  interval  freezes  the  robot  at  that  configuration  and 
signals  termination.  Presumably  such  a  mechanism  is  designed  to  avoid  damage  to 
the  robot  from  burning  out  its  joint  motors.  It  also  avoids  plans  with  long  delays 
while  the  planner  waits  for  the  motion  to  slide  again. 

The  role  of  time  in  constructing  EDR  regions  can  be  formalized  by  explicitly 
introducing  time  into  the  goal  specification.  Thus,  goals  become  regions  in  phase 
space-time;  points  in  goals  have  the  form  (x,  v,  t),  where  x  is  a  position,  v  a  velocity, 
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and  t  a  time.  Suppose  given  a  goal  G  in  generalized  configuration  space,  we  form  a 
phase  space-time  goal  which  is  the  product  of  n~^(G)  with  a  compact  time  interval. 
It  seems  that  the  EDR  axioms  are  satisfiable  by  EDR  regions  which  have  the  form 
of  a  product  of  (6)  with  a  compact  time  interval.  More  study  is  require  1. 

One  also  can  conceive  of  alternative  models  for  sticking  behavior,  ff,  is  all 
points  in  the  weak  but  not  strong  preimage  such  that  sticking  might  occm  there. 
Note  that  we  cannot  guarantee  that  sticking  will  occur,  since  then  the  point  would 
not  be  in  the  weak  preimage.  We  could  assume  a  probabilistic  distribution  of  control 
velocities  in  Bee-  In  this  case  we  could  infer  that  eventually,  given  an  unbounded 
amount  of  time,  a  motion  will  be  commanded  which  will  caiise  sliding  away  from 
any  point  in  Hg  at  which  a  trajectory  originating  in  R  sticks.  In  this  case,  the 
trajectory  cannot  stick  forever  in  Hg.  I  don’t  think  robot  controllers  reliably  en¬ 
force  probabilistic  distributions  of  commanded  velocities,  even  if  “dithering”  control 
strategies  are  employed.  Even  if  they  could,  this  model  of  sticking  makes  life  easier, 
since  it  essentially  eliminates  the  possibility  of  sticking  forever  in  P  —  P.  We  will 
not  make  this  assumption  here.  It  does  not  address  with  the  problem  of  “looping 
forever”  within  P  —  P  in  finite  environments.  It  seems  that  time-out  termination 
predicates  and/or  velocity  thresholding  must  be  used  to  solve  the  looping  problem. 
Both  solutions  seem  inelegant;  the  issue  is  subtle  and  should  be  addressed  further 
in  future  research  (see  chapter  VI). 

8.3.1  Summary 

When  the  goal  is  specified  in  phase  space-time  as  the  product  of  a  cylinder  over 
a  generalized  configuration  space  goal  with  a  compact  time  interval,  our  geometrical 
characterization  of  EDR  satisfies  the  EDR  axioms.  Without  time,  or  with  goals  of 
the  form  ir~^(G)  x  [<,  oo),  the  definition  of  H  does  not  completely  fulfill  the  EDR 
axioms.  This  is  because  it  is  possible  for  motions  sticking  in  Hg  to  eventuEilly  slide 
into  the  goal,  violating  the  principle  that  no  motion  should  be  terminated  as  a 
failure  while  serendipitous  goal  achievement  is  still  possible. 


III.  Multi-Step  Strategies 


In  this  chapter  we  explore  multi-step  strategy  construction.  Now,  in  principle, 
having  reduced  both  model  error  and  EDR  to  essentially  “preimage-theoretic”  equa¬ 
tions,  multi-step  strategies  could  be  synthesized  by  solving  these  preimage  equa¬ 
tions.  While  this  is  proved  or  at  least  implicit  in  previous  work  [LMT,Mason,E,D], 
it  is  far  from  obvious;  furthermore,  there  are  almost  no  published  examples  of  such 
strategies.  For  this  reason  we  begin  by  presenting  a  worked-out  example  of  a  motion 
plain  using  preimages.  The  motion  problem  is  grasi>  ^entering  for  a  robot  gripper 
in  the  presence  of  model  error.  Both  guaranteed  and  EDR  strategies  are  found  by 
solving  the  preimage  equations. 

Preimages  are  a  key  imderlying  tool  for  the  geometric  EDR  theory,  and  the 
[LMT]  framework  is  in  some  sense  a  “tmiversal”  method  for  synthesizing  multi-step 
strategies.  However,  the  technique  of  solving  the  preimage  equations  is  not  com¬ 
putational.  For  this  reason,  we  introduce  a  construction  called  the  push-forward. 
Roughly  speaking,  the  push-forward  is  that  subset  of  the  forward  projection  where 
the  motion  can  terminate.  Since  push-forwards  address  termination  whereas  for¬ 
ward  projections  do  not,  we  may  regard  them  as  “dual”  to  preimages.  That  is, 
push-forwards  are  to  forward  projections  as  preimages  zire  to  backprojections.  Sec¬ 
ond,  the  push-forward  permits  us  to  develop  rather  simple  algorithms  for  planning 
multi-step  strategies.  These  algorithms  have  been  implemented  in  LIMITED.  While 
the  push-forward  method  for  multi-step  strategy  synthesis  is  algorithmic,  it  is  less 
general  than  the  full  preimage  method  (solving  the  preimage  equations).  We  char¬ 
acterize  the  loss  of  power  in  push-forward  algorithms. 

In  chapter  I  we  presented  two  EDR  plans  generated  by  LIMITED.  These  were 
the  peg-in-hole  insertion  strategy  with  model  error,  and  the  ge£ir-meshing  plan. 
Both  were  two-step  plans.  We  will  go  into  more  detail  in  describing  how  these 
plans  were  generated.  The  peg-in-hole  plan  used  push-forward  techniques.  The 
gear  plan  used  a  seemingly  unrelated  technique  called  failure  mode  analysis.  We 
describe  failure  mode  analysis  and  algorithms  for  computing  it. 

Next,  we  will  present  a  view  of  multi-step  strategies  which  essentially  unifies  all 
these  techniques.  This  is  called  the  “weak”  EDR  theory.  The  motivation  behind  this 
theory  is  that  when  a  motion  terminates  ambiguously,  a  subsequent  motion  may  be 
synthesized  which  disambiguates  the  success  or  failure  of  the  first.  Oddly  enough,  it 
is  not  necessary  for  either  motion  individually  to  satisfy  the  EDR  axioms.  However, 
when  taken  together,  the  two-motion  plan  can  often  be  considered  “equivalent”  to 
a  one-step  EDR  strategy. 

The  weak  EDR  theory  effectively  defines  some  laws  of  “composition”  that  per¬ 
mit  two  single-step  plans  to  be  concatenated  into  a  two-step  plan  satisfying  the 


EDR  axioms.  Hence  it  is  often  possible  to  construct  multi-step  plans  that  are  EDR 
plans  “globally”  although  not  “locally”.  That  is,  considered  as  entire  plans,  they 
satisfy  the  EDR  axioms;  this  is  the  “global”  condition.  However,  “locally”  they 
are  not  EDR  plans,  in  that  no  single  step  is  an  EDR  strategy.  The  key  to  pasting 
together  non-EDR  plans  to  make  a  global  EDR  strategy  lies  in  defining  certain 
local  “niceness”  conditions  for  how  plans  must  mesh.  These  are  called  the  linking 
conditions. 

When  we  cross-reference  figures  in  another  chapter,  we  denote  this  by  11.29  for 
figure  29  in  chapter  II,  for  example. 

We  remind  the  reiider  that  starred  sections  may  be  skipped  if  desired. 


9.  Planning  using  Preimages:  A  Detailed  Example 


In  this  section  we  show  how  the  [LMT]  framework  can  be  used  to  synthesize 
multi-step  strategies.  Here  are  the  key  points  of  this  section: 

•  In  principle,  multi-step  plans  may  be  foimd  by  solving  a  family  of  preimage 
equations. 

•  While  this  was  proved  by  [LMT, Mason, E],  it  is  not  obvious  how  to  effect  the 
solution.  This  example  intends  to  elucidate  the  process. 

•  The  technique  is  general  enough  to  plan  EDR  strategies  under  model  error, 
once  we  have  cast  both  the  problem  of  planning  with  model  error  and  the  EDR 
problem  in  an  essentially  “preimage-theoretic”  form,  as  in  [D]  and  chapter  II. 

•  However,  the  technique  of  solving  the  preimage  equations  is  not  algorithmic. 

Furthermore,  preimages  are  a  key  underlying  tool  for  the  geometric  EDR  the¬ 
ory.  It  is  necessary  to  maJce  further  acquaintance  with  preimages  in  order  to  con¬ 
tinue  our  development  of  the  EDR  framework.  To  that  end,  this  section  presents 
a  worked-out  example  of  a  motion  plan  using  preimages.  The  motion  problem  is 
grasp-centering  for  a  robot  gripper  in  the  presence  of  model  error.  The  example 
illustrates  the  use  of  the  preimage  framework  to  derive  a  multi-step  motion  strategy 
in  the  presence  of  model  error.  The  strategy  employs  time-sensing  and  force-sensing. 
This  discussion  is  designed  both  as  a  tutorial  in  solving  preimage  equations  for  a 
motion  plan,  and  as  an  introduction  to  the  planning  of  multi-step  strategies. 

9.1.  Example:  Planning  Grasp-Centering  using  Preimages 

The  remainder  of  this  thesis  builds  on  the  preimage  framework  to  develop  the 
EDR  theory.  To  make  the  framework  more  accessible,  we  provide  here  a  fairly 
detailed  description  of  a  motion  planning  problem  using  preimages. ^ 

We  are  now  ready  to  work  an  example.  We  solve  a  particular  motion  planning 
problem  with  model  error  by  solving  the  preimage  equations.  This  example  provides 
an  illustration  of  planning  using  preimages.  For  simplicity,  we  initially  address  only 
the  problem  of  finding  a  guaranteed  strategy.  Finding  EDR  strategies  in  this  domain 
is  discussed  eifterwards. 

Consider  the  grasp-centering  problem  shown  in  fig.  1.  The  taisk  is  to  center 
the  robot  gripper  over  the  block  D.  The  gripper  can  translate  but  not  rotate  in  the 
plane.  In  its  start  position,  the  gripper  is  somewhere  over  D,  such  that  the  bottom 

'This  problem  arose  in  discussions  with  Tomas  Lozano-Perez,  Jolin  Canny,  and  Mike 
Erdmann. 


of  the  fingers  FA  and  FB  are  below  the  top  of  D.  The  width  of  D  is  unknown, 
but  must  be  less  than  the  distance  between  FA  and  FB.  We  aissume  D  is  fixed  (it 
cannot  be  accidentally  pushed). 

Hence  we  can  regard  this  as  a  planning  problem  with  model  error.  C  is  taken 
to  be  the  cartesian  plane,  and  7  is  a  boimded  interval  of  the  positive  reals.  Our 
first  question  is,  what  does  the  generalized  configmation  space  look  like?  This  is 
easily  answered  by  considering  the  motion  planning  problem  in  fig.  2.  The  problem 
is  to  find  a  motion  strategy  for  a  point  robot  so  that  it  can  achieve  a  goal  exactly 
halfway  between  the  blocks  A  and  B.  The  distance  a  between  A  and  B  is  unknown 
and  positive.  The  point  robot  is  known  to  start  between  A  and  B.  Again,  the  point 
can  translate  in  the  plane.  The  distance  a  is  the  model  error  parameter.  It  is  easy 
to  see  that  the  problems  in  figs.  1  and  2  are  equivalent. 

However,  we  alrerudy  know  what  the  generalized  configuration  space  for  fig.  2 
looks  like.  It  was  discussed  at  length  in  chapter  II,  and  is  shown  in  fig.  11.14.  Hence 
our  example  is  a  planning  problem  in  a  familiar  generalized  configuration  spaice. 

Next,  we  assume  that  the  robot  has  perfect  control,  perfect  velocity  sensing, 
and  a  perfectly  accmate  sense  of  time.  However,  it  has  infinite  position  sensing 
error.  ^ 

Now,  since  the  gripper  starts  over  D  with  the  bottom  of  the  fingers  below  the 
top  of  D,  and  since  the  robot  has  perfect  control,  it  suffices  to  consider  the  x  axis 
of  C.  Since  the  y  axis  can  be  ignored,  we  develop  our  example  in  the  plane,  that 
is,  in  the  generalized  configuration  space  where  C  and  7  are  both  one-dimensional. 
This  2D  generalized  configuration  space  is  shown  in  fig.  3,  which  is  essentially  an 
x-7  cross-section  of  fig.  11.16,  holding  y  constemt  with  a  constrained  to  be  positive. 
In  fig.  3,  L  and  R  are  left  and  right  obstacle  edge  boundaries  generated  by  A  and 
B.  The  goal  is  the  line  in  free-space  bisecting  L  and  R.  The  start  region  T  is  the 
triangular  region  in  free-space  between  L  and  R.  (T  is  the  convex  hull  of  L  and  R). 

Now,  since  motion  across  7  is  not  permitted,  all  motions  are  parallel  to  the  x 
axis,  that  is  to  say,  horizont2il  in  fig.  3.  There  are  only  two  kinds  of  motions  the 
plzinner  can  command.  Let  -f  denote  a  motion  to  the  right,  and  —  a  motion  to  the 
left.  We  assume  the  robot  has  perfect  control  over  the  magnitude  as  well  as  the 
direction  of  the  commanded  velocity. 

See  fig.  3.  Now,  if  q  is  a  point  on  the  7  axis,  let  Ea  be  the  point  on  the 
left  obstacle  edge  L  with  7  coordinate  a.  We  will  denote  the  collection  of  all  such 
points  on  L  by  {  }.  Let  Sq  denote  the  maximal  line  segment  within  T  containing 

Ea  and  parallel  to  G.  Formally,  if  Eq  has  coordinates  (i,a),  then  So  is  the  line 
segment  extending  from  Ea  to  (x,d)  where  d  is  an  upper  bound  on  the  distance 
between  A  and  B.  We  denote  the  collection  of  all  lines  5o  by  {  5o  }. 

^This  example  is  easily  generalized  to  non  zero  control,  time-sensing,  and  force-sensing 
error,  and  finite  position-sensing  error.  This  requires  giving  the  goal  non-empty  interior, 
however. 


Fig.  1.  The  grasp  centering  problem.  The  width  of  the  block  D  on  the  table, 
and  the  position  of  the  gripper  are  only  known  approximately. 

At  this  point  we  are  prepared  to  derive  a  motion  strategy  for  centering  the 
grasp,  that  is,  for  attaining  G  from  T.  The  strategy  has  three  stepw.  The  termination 
conditions  for  the  motions  involve  time-  and  force-sensing.  Here  is  the  motion 
strategy  in  qualitative  terms; 

Strategy  Guarantee- Center 

1.  Command  a  motion  to  the  right.  Terminate  on  the  right  edge  R  baaed  on  force 
senaing. 

2.  Command  a  velocity  of  known  magnitude  to  the  left.  Terminate  when  in  contact 
mth  the  left  edge  L,  uaing  force  aenaing.  Meaaure  the  elapaed  time  of  the  motion. 
Compute  the  distance  traversed.  This  gives  exact  knowledge  of  where  the  motion 
terminated  on  L.  The  effect  of  this  step  ia  to  meaaure  the  distance  a  between  the 
blocks. 

3.  Move  distance  y  to  the  right,  terminating  in  G  based  on  time  aenaing. 
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Fig.  2.  An  equivalent  problem.  A  point  robot  must  be  navigated  halfway  between 
the  blocks  A  and  B.  The  distance  between  A  and  B  is  not  known.  The  robot 
has  force  sensing,  and  a  clock.  However,  it  has  poor  position  sensing.  We  regard 
(7  as  and  J  as  the  bounded  interval  (0,d]  for  d  positive.  The  generalized 
configuration  space  for  this  problem  is  the  same  as  in  fig.  II.'*',  for  the  positive 
values  in  J. 

We  now  derive  this  strategy  by  solving  the  preimage  equations  for  the  motion 
planning  problem. 

First,  note  that  if  the  run-time  executive  knows  that  the  robot  is  inside  a 
particular  Sa,  then  G  can  be  reliably  £u:hieved  by  commanding  a  motion  to  the 
right.  Since  the  robot  has  perfect  control  and  time  sensing,  the  motion  cam  be 
terminated  after  moving  distance  j,  that  is,  exactly  when  the  line  G  is  achieved. 
Using  the  preimage  notation,  we  write  this  as 

=  (1) 
Next,  we  tadce  the  collection  {  5a  }  as  a  set  of  subgoals,  and  try  to  find  a  motion 
that  can  recognizably  attain  this  collection,  and,  furthermore,  can  distinguish  which 


Fig.  3.  Assuming  that  the  gripper  fingers  are  initially  lower  than  the  top  of  the 
block  Dy  the  y  dimension  can  effectively  be  ignored.  This  allows  us  to  examine  a 
cross-section  of  fig.  11.16.  We  treat  C  as  the  x  axis  of  motion  freedom,  yielding 
a  2D  C  X  J  planning  space.  L  and  R  are  obstacle  boundaries  in  generalized 
configuration  space  .  The  goal  is  the  bisector  G  between  L  and  R  in  free-space. 
The  start  region  T  is  the  triangular  region  between  L  and  R.  Ea  is  a  point  on 
L.  5a  is  a  line  in  T  puallel  to  G  and  containing  So- 


Sa  the  motion  achieves.  Consider  a  leftward  motion  starting  from  anywhere  on  the 
right  edge  R.  The  robot  does  not  know  where  on  R  the  motion  starts,  however.  To 
recognizably  achieve  some  Sa,  such  a  motion  should  move  leftward,  and  terminate 
when  force-sensing  indicates  that  L  has  been  reached.  If  the  termination  predicate 
measures  the  elapsed  time  of  the  motion,  and  knows  the  magnitude  of  the  com¬ 
manded  velocity,  then  it  can  recognize  which  point  Ef,  has  been  reached,  and  hence 
which  subgosd  Sa  has  been  achieved.  Writing  this  down  in  preimage  equations, 

^-,*({5,))  =  ?-, «({£.})  =  «.  (2) 

Finally,  the  right  edge  R  may  be  achieved  from  anywhere  within  the  start  region 


T  by  moving  rightwEird,  and  terminating  when  force  sensing  indicates  contact.  This 
is  simply 


P+.r(ii)  =  T.  (3) 

It  is  instructive  to  examine  the  termination  conditions  for  motions  (l)-(3). 
In  motion  (1),  only  the  time-indexed  forward  projection  F+(5a,i)  is  relevant  to 
deciding  termination.  The  motion  terminates  when  (5a,  <)  C  G.  Motion  (3)  can 
be  terminated  using  pure  force  sensing.  It  could  edso  be  terminated  using  time, 
since  there  exists  some  t  for  which  P+(T,  t)  =  R.  In  motion  (2),  both  force  sensing 
and  time  are  required  to  terminate  within  a  distinguishable  Ea-  The  general  form 
of  the  termination  condition  for  all  three  cases  is  as  follows.  Recall  that  in  general, 
the  termination  predicate  has  the  form 

F9{U,t)  n  X  c 

for  a  goal  G^  and  a  start  region  U .  (Assume  that  all  subgoals  have  been  hfted  into 
phase  space;  see  sec.  8.2).  In  our  case,  position  sensing  error  is  infinite,  so  Bep{i)  is 
C  X  J.  Let  us  denote  (C  x  J)  x  Btvi^)  simpler  expression  Bv{t)-  Then  the 

termination  conditions  for  motions  (l)-(3)  are  as  follows.  For  the  first  motion  (3), 
to  terminate,  we  must  have 


F+{T,t)r\B„it)cR.  (4) 

For  the  second  motion  (2)  to  terminate,  we  must  have 

F_(R,t)nB„(f)c5„  (5) 

for  some  Sa.  We  think  of  the  termination  predicate  as  “returning”  this  5a.  Finally, 
for  termination  of  the  last  motion  (1),  we  must  have 

F+(5a)nP„(f)cG,  (6) 

where  the  Sa  in  (6)  is  the  same  as  the  one  returned  by  the  termination  predicate 
after  the  second  motion  as  the  satisfying  assignment  for  (5). 

Finally,  note  that  time  is  the  source  of  some  complexity  in  this  example.  This 
complexity  might  be  removed  by  employing  a  distance  sensor  instead.  The  output 
of  such  a  sensor  could  be  modeled  as  position  sensing  in  J.  The  sensing  action  in  J 
would  entail  meeisuring  the  distance  between  A  and  B.  This  relaxes  the  assumption 
of  no  position  sensing  in  the  J  dimensions,  but  such  modification  to  the  generalized 
configuration  space  framework  is  trivial.  With  this  modification,  Bep  is  simply 
regarded  as  a  product  of  a  position  sensing  ball  in  C  and  a  position  sensing  set  in 


This  concludes  the  example.  We  have  shown  how  to  derive  a  multi-step  guar¬ 
anteed  motion  strategy  in  the  presence  of  model  error.  The  strategy  was  derived 
by  solving  the  preimage  equations  in  generalized  configuration  space  for  the  motion 
plan.  These  preimage  equations  made  the  role  of  time-  and  force-sensing  explicit 
in  deriving  conditions  for  distinguishable  termination  in  a  collection  of  subgoals. 
With  this  example  in  mind,  the  reader  should  be  well-equipped  to  wade  into  the 
remainder  of  this  thesis. 

9.1.1  An  EDR  Strategy  for  Grasp-Centering 

We  now  generalize  the  grasp-center  example  and  show  how  to  develop  an  EDR 
strategy  for  this  problem. 

Assume  that  the  radius  of  position  sensing  uncertainty  is  larger  than  the 
diameter  of  T,  but  not  infinite.*  Furthermore,  assume  that  a,  the  distEince  between 
A  and  B,  can  be  zero  (but  not  negative)  in  the  above  example.  That  is,  D  can  be 
too  big  to  grasp.  Hence  the  hole  between  A  and  B  can  close  up,  as  in  fig.  11.16. 
Assume  that  the  gripper  starts  above  the  height  of  the  block  D,  in  the  circular 
region  R  in  fig.  11.15.  Generalize  the  discussion  of  preimages  above  to  describe  an 
EDR  strategy  using  preimages.  We  will  need  to  consider  the  y  dimension  of  motion 
freedom  as  well,  in  the  3D  generalized  configuration  space  shown  in  fig.  11.16,  but 
only  the  non-negative  a  in  J.  Note  that  EDR  is  “required”  here,  since  if  a  can  be 
zero,  there  exists  no  guaranteed  strategy. 

Let  us  rename  the  circular  start  region  in  fig.  11.15  to  be  U,  and  continue 
to  use  R  for  the  right  edge  in  fig.  3.  Assume  that  the  x-J  slice  of  generalized 
configuration  speice  in  fig.  3  is  taken  at  y  =  0,  i.e.,  at  the  level  of  the  table,  and 
that  under  the  commanded  motion  vj,  shown  in  fig.  11.15,  sliding  occurs  on  all 
horizontal  and  vertical  surfaces.  However,  clearly  sticking  will  occur  under  uj  on 
the  concave  left  edge  L  between  A  and  the  table. 

Now,  let  if  be  as  in  fig.  11.24.  Here  is  the  EDR  strategy  in  qualitative  terms: 

Strategy  EDR- Center 

El.  From  U ,  command  the  motion  Vg.  Terminate  on  the  left  edge  L  based  on 
sticking,  or  in  H  based  on  time. 

E2.  If  H  is  attained,  signal  failure.  Otherwise,  go  to  step  (1)  of  strategy  Guarantee- 
Center. 

*This  assumption  is  not  necessary,  but  it  simplifies  our  discussion  somewhat. 


Now,  since  Hs  =  0,  the  preimage  equation"*  (II. 3a)  for  step  (El)  simply  reduces 
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P,,u{{I'<H))  =  U.  (7) 

At  this  point,  the  remainder  of  the  strategy  may  be  developed  in  the  x-J  slice 
shown  in  fig.  3.  To  finish  the  preimage  ch2iracterization  of  the  EDR  strategy,  we 
must  replace  eq.  (3),  which  characterizes  the  first  step  (1)  of  strategy  Guarantee- 
Center,  by 

P+MR)  =  £■  (8) 

Note  that  (8)  is  actually  a  logical  consequence  of  (3),  since  T  is  a  subset  of 
T.  Analogously,  (4)  must  be  changed  by  replacing  T  by  L.  The  remainder  of  the 
preimage  equations  (l)-(2)  and  (5)-(6)  remain  unchanged. 

9.2.  Solving  the  Preimage  Equations  is  General  but  Not  Computational 

This  example  shows  how  multi-step  EDR  strategies  under  model  error  can  be 
generated  by  solving  a  family  of  preimage  equations.  However,  the  technique  is  not 
an  algorithm.  We  do  not  claim  that  such  an  algorithm  could  not  be  developed,  but 
merely  that  as  described  above  and  in  [LMT, Mason, E],  the  method  is  not  (yet)  com¬ 
putationally  effective.®  The  first  reason  it  is  non-computational  is  that  the  number 
of  subgoals  {Ea}  and  {  5a  }  is  infinite.  The  second,  and  more  important  reason  is 
that  solving  the  preimage  equation  is,  as  stated,  a  decision  problem  in  second-order 
set  theory.  Even  if  the  sets  are,  say,  algebraic,  this  theory  is  imdecidable.  However, 
there  may  exist  a  reformulation  of  the  problem  rendering  it  decidable.  Below  we 
describe  one  such  reformulation,  using  push-forwards,  which  can  be  iised  in  effect  to 
solve  certain  “simple”  preimage  equations  and  hence  to  generate  a  restricted  class 
of  EDR  plans. 


^See  sec.  8.2. 

®However,  note  that  Erdmann’s  techniques  of  approximating  preimages  by  backprojec- 
tions  may  lead  toward  a  fully-algorithmic  method. 
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10.  Push-Forwards:  A  Simple  Generalization  to  n-Step  EDR 
Strategies 

The  generalized  preimage  framework  [LMT, Mason, E,D]  gives  a  kind  of  “uni¬ 
versal”  method  for  generating  multi-step  EDR  strategies.  However,  the  technique 
of  solving  the  preimage  equations  is  not  algorithmic — it  is  more  like  doing  a  proof 
by  hand.  For  this  reason,  we  introduce  the  push-forward  technique  for  synthesizing 
multi-step  strategies.  While  considerably  less  general  than  solving  the  full  preimage 
equations,  it  leads  to  rather  simple  multi-step  strategy-generation  algorithms,  which 
were  implemented  in  LIMITED.  The  push-forward  technique  is  powerful  enough  to 
generate  an  EDR  plan  for  the  peg-in-hole  insertion  strategy  with  model  error  de¬ 
scribed  in  chapter  I.  However,  it  is  not  generzJ  enough  to  solve  all  steps  of  the 
grasp-centering  example  discussed  above.  This  gives  us  a  measure  of  the  relative 
power  of  push-forward  vs.  preimage  equation  techniques. 

This  section  first  discusses  the  push-forward  technique  for  synthesizing  n-step 
EDR  strategies  in  some  detail.  When  we  cross-reference  figures  in  another  chapter, 
we  denote  this  by  11.29  for  figure  29  in  chapter  II,  for  exzimple. 

We  first  review  the  “Twin  Universe”  example  (3)  (figs.  11.29,11.30),  highlighting 
a  subtle  recognizability  issue  not  emphasized  in  the  prelude.  However,  this  review 
may  be  skipped  at  first  reading  if  you  already  have  example  (3)  firmly  in  mind. 

A  Review  of  the  “Twin  Universe”  Example  (3) 

Consider  fig.  11.29.  Here  there  are  two  possible  universes,  both  in  the  plane,  so  J  is 
the  two  element  discrete  set,  {  1,2  }.  The  start  region  is  the  union  of  Ri  in  imiverse 
1,  and  R2  iu  universe  2.  The  goal  exists  in  universe  1  but  not  in  universe  2.  There 
is  no  one-step  EDR  strategy  which,  from  the  start  region,  can  guarantee  to  achieve 
G  or  recognize  that  we  are  in  universe  2.  In  particular,  there  is  no  one-step  EDR 
strategy  which  can  be  derived  from  the  motion  Vg. 

However,  there  clearly  exist  multi-step  EDR  strategies.  We  will  construct  one 
as  follows.  Recall  that  to  construct  one-step  EDR  strategies,  we  took  eis  data  a 
goal,  a  start  region  R,  a  commanded  motion  9,  and  the  preimage  of  the  goal  under 
9.  Given  this  data  we  constructed  an  EDR  region.  From  the  EDR  region,  we 
attempted  to  construct  an  EDR  strategy  that  achieved  the  distinguishable  union  of 
the  goal  or  the  EDR  region.  Now,  why  does  this  fail  in  fig.  11.29?  To  answer  this 
question,  let  us  consider  what  the  motion  9  was  supposed  to  achieve  in  universe 
1.  There  is  an  8-step  plan  in  universe  1  which  recognizably  achieves  G  from  start 
region  Rj.  It  is  obtained  by  back-chaining  preimages  in  universe  1.  The  plan  moves 
from  Ri  to  the  region  Si  under  Vg.  Then  it  slides  along  the  top  surface  to  vertex 
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/.  Next  it  slides  to  vertex  e.  It  slides  to  the  successive  vertex  subgoals  d  through 
a,  and  then  a  horizontal  sliding  motion  au:hieves  the  goal  G. 

The  strategy  9  is  guaranteed  to  achieve  the  surface  Si  from  start  region  i?i. 
Suppose  we  try  to  extend  it  to  an  EDR  strategy  with  start  region  the  union  of  Rj 
and  i?2  •  The  EDR  region  is  then  simply  the  (cylinder  over  the)  forward  projection 
of  the  “bad”  region,  F0{R2).  (See  fig.  11.29).  There  is  no  way  that  the  termination 
predicate  cam  distinguish  between  the  forward  projection  of  Ri  and  the  forward 
projection  of  R^ ,  hence  no  EDR  strategy  from  9  exists. 

We  can  easily  construct  a  2-step  EDR  strategy,  however.  First,  we  execute 
motion  9  from  the  union  of  Ri  and  R2.  This  achieves  a  motion  into  Sj  in  universe 
1,  or  into  S2  in  rmiverse  2.  The  termination  predicate  cannot  distinguish  which  has 
been  attained.  Suppose  the  second  motion  in  the  8-step  plan  is  (see  fig.  11.29), 
and  is  guaranteed  to  achieve  the  vertex  subgoal  /  from  start  region  Si .  We  will  try 
to  construct  an  EDR  strategy  out  of  this  second  motion.  Take  as  data:  the  subgoal 
/,  the  start  region  Si  U  S2,  the  “southwest”  motion  rl),  and  the  preimage  of  /  under 
j/>.^  The  EDR  region  for  these  data  is  the  forward  projection  of  S2  under  rp  (see 
fig.  11.30).  Presumably  this  EDR  region  is  (eventually)  distinguishable  from  /,  and 
so  we  have  constructed  an  EDR  strategy  at  the  second  step.  After  executing  the 
second  step,  we  either  terminate  the  motion  eis  a  failure,  or  proceed  to  vertex  e,  and 
eventually  to  the  goal. 

10,1.  Generalization:  Push-Forwards 

Now,  let  us  attempt  to  capture  the  salient  aspects  of  the  n-step  EDR  strategy 
construction.  We  take  as  data  an  n-step  plan,  with  start  region  i?i.  The  actual 
start  region  is  some  larger  region,  say,  R.  Above,  we  had  R  as  the  union  of  Ri  and 
R2-  The  first  motion  in  the  plan  is  guaranteed  to  achieve  some  subgoal  Si  from  i2i . 
Using  this  first  motion  from  start  region  i?,  we  try  to  construct  an  EDR  region  Hi, 
and  a  one-step  EDR  strategy  that  either  achieves  Si  or  signals  failure  by  achieving 
Hi .  If  this  succeeds,  we  are,  of  course,  done. 

Suppose  we  cannot  distinguish  between  Hi  and  Si-  In  this  case,  we  want  to 
execute  the  first  motion  “anyway,”  and  terminate  “somewhere”  in  the  union  of  Si 
and  Hi-  The  termination  predicate  caimot  be  guaranteed  to  distinguish  which  goal 
hais  been  entered. 

This  “somewhere”  is  called  the  push-forward  of  the  first  motion  from  R.  The 
push-forward  is  a  fimction  of  the  commanded  motion  9,  the  actual  start  region 

'While  5i  is  the  preimage  of  /  under  0  with  respect  to  start  region  5i ,  the  preimage 
with  respect  to  the  entire  forward  projection  of  Si  U  S2  includes  the  top  edge  between 


R,  the  region  Ri  from  which  9  is  guaranteed,  and  the  subgoal  A  particular 
type  of  push-forward  is  defined  formally  in  an  appendix;  we  describe  it  informally 
below.  In  example  (3),  the  push-forward  (under  9)  of  R2  is  S2.  The  push-forward 
of  i?i  U  R2  is  Si  U  Sj.  The  push-forwzu-d  ■  similar  to  a  forward  projection,  except 
that  it  addresses  the  issue  of  termination.  In  example  (3),  informally  spealdng,  the 
push-forward  from  the  region  R  (under  some  commanded  motion  9)  is  the  result  of 
executing  9  from  R  ajid  seeing  what  happens.  It  is  defined  even  when  the  strategy 
9  is  only  guaranteed  from  some  subset  (/2i)  of  R. 

Having  terminated  in  the  push-forward  of  R  (the  union  of  Si  and  S2  above), 
we  next  try  to  construct  a  one-step  EDR  strategy  at  the  second  motion  of  the  n- 
step  plan.  The  data  are:  the  next  subgoal  Ti  after  Si  in  the  plan,  the  actual  start 
region  Si  U  S2,  the  second  commanded  motion  in  the  plan,  and  the  preimage  of  Ti 
under  this  motion.^  This  defines  a  formal  procedure  for  constructing  n-step  EDR 
strategies.  At  each  stage  we  attempt  to  construct  a  one-step  EDR  strategy;  if  this 
fails,  we  push-forward  and  try  again. 

Actually,  this  description  of  the  procedure  is  not  qiiite  complete.  At  each  step 
we  construct  the  EDR  region  as  described.  However,  the  one-step  strategy  we  seek 
must  achieve  the  distinguishable  union  of  the  EDR  region  and  all  unatiained  subgoals 
in  the  plan.  That  is,  the  EDR  motion  must  distinguishably  terminate  in  the  EDR 
region,  or  the  next  subgoal,  or  any  subsequent  subgoal.  This  allows  serendipitous 
skipping  of  steps  in  the  plan. 

By  considering  different  data,  that  is,  quantifying  over  all  motions  at  each 
branch  point  of  the  n-step  strategy,  we  can  in  principle  consider  all  n-step  strategies 
and  define  non-directionaJ  EDR  strategies.  This  is  at  least  as  difficult  as  computing 
n-step  non-directional  preimages.  If  we  wish  to  consider  plans  of  different  lengths, 
we  must  also  quantify  over  all  n.  Needless  to  say,  the  branching  factor  in  the 
back-chaining  search  would  be  quite  large. 

10.2.  More  on  the  Push-Forward 

The  problem  of  defining  the  push-forward  may  be  stated  informally  as  follows: 
“Where  should  the  motion  be  terminated  so  that  later,  after  some  additional  number 
of  push-forwards,  a  one-step  EDR  strategy  may  be  executed.” 

Many  different  push-forwards  can  be  defined.  Using  the  notation  above,  note 
the  motion  is  not  even  guaranteed  to  terminate  when  executed  from  R:  it  is  only 
guaranteed  from  Ri-  This  means  that  velocity- thresholding  and  time  may  be  neces¬ 
sary  in  the  termination  predicate.  There  are  other  difficulties:  for  example,  a  priori 

^Of  course,  it  also  depends  on  the  termination  predicate,  sensing  and  control  character¬ 
istics,  etc. 

^The  preimage  is  with  respect  to  the  forward  projection  of  the  actual  start  region  5iU52. 


it  is  not  even  necessary  that  entry  into  the  union  of  the  subgoal  Si  and  the  EDR 
region  Hi  be  recognizable.  Thus  defining  the  push-forward  is  equivalent  to  defining 
where  in  Si  U  Hi  the  motion  can  and  should  be  terminated.  (However,  see  note  (1) 
in  the  appendix  ). 

Depending  on  that  push-forward  is  employed,  we  may  or  may  not  obtain  an  n- 
step  EDR  strategy.  It  is  possible  to  define  constraints  on  the  push-forward  that  must 
be  satisfied  to  ensure  that  a  strategy  will  be  found  if  one  exists.  These  constraints 
are  given  in  an  appendix.  While  in  the  appendix  we  can  give  equations  that  the 
push-forward  must  satisfy,  at  this  time  a  constructive  definition  is  not  known.  This 
situation  is  similar  to,  and  possibly  harder  than  the  problem  of  solving  the  general 
pre-image  equation. 


10.3.  An  Approximation  to  the  Push-Forward 
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We  may  have  to  approximate  the  desired  push-forward.  We  give  such  an  ap¬ 
proximation  here.  In  general,  it  does  not  satisfy  the  constraints  given  in  the  ap¬ 
pendix.  We  provide  it  to  show  what  the  push-forwards  alluded  to  above  are  like. 
Such  approximate  push-forwards  may  prove  useful  in  approximating  the  desired 
push-forward.  The  issue  deserves  more  study.  Since  this  approximate  push-forward 
is  incomplete,  the  reader  should  consider  its  description  here  as  illustrative  of  the 
research  problem,  and  not  as  an  endorsement. 

The  push-forward  employed  in  example  (3)  was  formed  by  “executing  the  strat¬ 
egy  anyway,  and  seeing  where  it  terminated.”  How  do  we  formalize  this  idea? 
Consider  the  termination  predicate  as  a  function  of  the  starting  region,  the  initied 
sensed  position,  the  commanded  velocity,  the  gO£il(s),  and  the  sensor  values.  The 
sensor  values  are  changing;  the  predicate  monitors  them  to  determine  when  the  goed 
has  been  reached.  Now,  if  the  termination  predicate  “knew”  that  in  example  (3) 
the  start  region  wais  the  union  of  Ri  and  /22,  then  the  first  motion  strategy  9  could 
never  be  terminated;  the  predicate  could  never  ensure  that  the  subgoal  Si  hewi  been 
reached.  This  is  simply  because  Si  and  Sj  are  indistinguishable.  But  if  we  “lie”  to 
the  termination  predicate  and  tell  it  that  the  motion  really  started  in  Ri ,  then  the 
predicate  will  happily  terminate  the  motion  in  Si  U  S2,  thinking  that  Si  has  been 
achieved.  Viewing  the  termination  predicate  as  a  function,  this  reduces  to  calling 
it  with  the  “wrong”  arguments,  that  is,  applying  it  to  Ri  instead  of  Ri  U  ^2-  The 
push-forward  we  obtain  is  “where  the  termination  predicate  will  halt  the  motion 
from  all  of  Ri  U  R21  thinking  that  the  motion  originated  in  Ri"  S2  is  obtained  as 
the  set  of  places  outside  of  Si  where  the  hed-to  termination  predicate  can  halt. 

Even  formalizing  the  construction  of  this  simple  push-forward  is  subtle;  de¬ 
tails  are  given  in  an  appendix.  While  this  approximate  push-forward  is  incomplete, 
it  does  suffice  for  a  wide  variety  of  EDR  tasks.  The  approximate  push-forward 
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captures  the  intuitive  notion  of  “trying  the  strategy  anyway,  even  if  we’re  net  guar¬ 
anteed  to  be  in  the  right  initial  region.”  It  is  incomplete  because  it  fails  to  exploit 
sufficiently  the  geometry  of  the  forward  projection  of  the  “bad”  region.  Better 
push-forwards  must  be  found;  this  one  is  merely  illustrative  of  the  problems. 

10.4.  Example:  Multi-step  EDR  Plan  for  Peg-in-Hole  with  Model 
Error 

The  advantage  of  the  push-forward  technique  is  that  it  can  be  made  computa¬ 
tional.  We  now  give  LiMITED’s  algorithm  for  generating  multi-step  strategies  using 
push-forwards,  and  describe  an  experiment  which  used  this  method. 

Recall  chapter  I,  figs.  6-16,  which  described  a  two-step  EDR  plan  for  a  peg- 


in-hole  plan  with  3  DOF  model  error.  Here  is  how  this  multi-step  strategy  was 
generated; 

Algorithm  Multi 

1.  First,  try  to  generate  a  one-step  EDR  strategy  using  the  algorithm  in  sec.  6.S. 
Suppose  this  fails.  Then: 

2.  Generate  a  commanded  velocity  Vg,  such  that  the  forward  projection  of  the  start 
region  intersects  the  goal  in  some  slice. 

3.  Compute  the  EDR  region  H  for  vj. 

4.  Compute  the  sticking  push-forward  of  the  motion,  R\  =  pushj(G  U  H). 

5.  Using  Ri  as  the  start  region,  generate  a  one-step  EDR  strategy  using  the  algo¬ 
rithm  in  sec.  6.3. 


Of  course,  in  LIMITED  the  computation  is  memoized  so  that  the  projection 
and  EDR  regions  computed  in  step  (1)  are  not  recalculated  in  steps  (2)  and  (3). 
Obvioiisly,  we  can  extend  this  algorithm  to  generate  longer  strategies  which  push- 
forward  several  times  and  finally  terminate  in  a  single-step  EDR  strategy. 

Now,  Limited  is  a  multi-resolution  planner.  The  algorithm  outlined  above 
generates  a  multi-step  strategy  at  a  single  resolution.  The  resolution  of  planning  is 
simply  the  set  of  a  values  in  which  slices  are  taken.  A  resolution  is  finer  than 
S2  if  it  contains  more  slices.  The  multi-resolution  outer  loop  works  like  this: 


Ml.  At  a  coarse  resolution,  generate  a  multi-step  EDR  strategy  di, . . .  using  the 
forward- chaining  single-resolution  algorithm  above. 

M2.  Select  a  finer  resolution.  Use  the  directions  di,...,9n  us  a  suggested  strategy 
and  attempt  to  verify  that  it  is  an  EDR  strategy  at  the  finer  resolution. 


V 


M3.  If  01,  ■■■  1  On  *■*  strategy  at  the  finer  resolution,  try  to  modify  it 

so  that  it  is,  by  using  di,...,0n  ns  suggested  directions,  and  searching  nearby 

directions  at  all  levels. 

The  process  terminates  when  the  resolution  is  finer  than  some  predetermined 
level."*  The  critical  slice  method  described  in  chapter  II  may  be  one  way  to  obtain 
such  an  a  priori  bound  and  know  that  it  is  sufficient.  In  LIMITED,  however,  the 
bound  is  a  user  input  because  otherwise  the  number  of  slices  required  would  be 
prohibitive. 

In  the  peg-in-hole  example  there  were  3DOF  of  model  error:  the  width  of  the 
hole,  the  depth  of  the  chamfers,  and  the  orientation  of  the  hole.  The  resolutions 
used  in  planning  the  two-step  strategy  were  as  follows; 

Rl.  Holding  orientation  fixed,  4  slices  of  the  depth  x  width  axes. 

R2.  Holding  orientation  fixed,  16  slices  of  the  depth  x  width  axes. 

R3.  Holding  orientation  fixed,  72  slices  of  the  depth  x  width  axes. 

R4.  100  slices  of  the  depth  x  width  x  orientation  axes. 

The  figures  show  details  of  the  slices  zmd  the  plan. 

10.5.  The  Loss  of  Power  with  Push-Forward  Techniques 

While  push-forwards  permit  us  to  develop  simple  algorithms  for  generating 
mvilti-step  strategies,  clearly  these  algorithms  are  theoretically  less  powerful  than 
solving  the  preimage  equations  in  full  generality.  We  now  attempt  to  give  an  intu¬ 
itive  characterization  of  the  loss  of  power.  In  particular,  push-forwards  are  general 
enough  for  the  peg-in-hole  EDR  strategy  with  model  error.  However,  they  are  not 
general  enough  to  generate  the  grasp-centering  plan.  We  now  discuss  where  in 
the  grasp-centering  example  the  push-forward  techniques  are  inadequate.  The  key 
point  is  this;  if  each  commanded  motion  and  termination  condition  could  be  non- 
deterministically  “guessed,”  and  a  push-forward  for  each  motion  and  termination 
condition  could  be  computed,  then  in  the  grasp-center  example  this  would  suffice 
to  generate  a  strategy.  However,  the  push-forward  algorithms  we  have  developed 
are  not  powerful  enough  to  do  this. 

First,  let  us  derive  the  push-forwards  of  each  motion  in  strategies  EDR- Center 
and  Guarantee-Center.  Recall  that  El  is  the  first  step  of  the  EDR  plan,  and  motions 
1,  2,  and  3  are  steps  in  the  (subsequent)  guaranteed  plan.  In  the  third  column  we 
note  whether  or  not  the  push-forward  technique  is  computationally  effective  for  tliis 
motion. 

‘*Or,  when  at  some  level,  no  EDR  strategy  can  be  found. 


Motion  Push  —  Forward  Computational? 
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G 
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The  push-forwards  for  motions  El  and  1  can  be  computed  using  the  algorithms 
of  sec.  6.2  and  algorithm  Multi  above.  In  motion  El,  L  may  be  found  using 
sticking  termination.  H  may  be  found  using  time,  or  position  and  force  sensing 
termination.  In  motion  1,  R  may  be  found  using  contact,  or  sticking  termination. 
However,  our  algorithms  cannot  compute  the  push- forward  {  ilo  which  contains 
an  infinite  number  of  components.  Furthermore,  we  have  not  developed  algorithms 
for  computing  push-forwards  based  on  time-termination  (except  for  elapsed  time 
termination,  of  the  form  “terminate  anytime  after  t  seconds”).  Thus  the  push- 
forward  G  for  the  last  motion  cannot  be  computed  by  our  algorithms  either. 

10.5.1  Discussion 

Let  us  pause  to  review.  We  first  described  a  fully-general,  but  non-computational 
technique  for  generating  multi-step  strategies.  This  method — solving  the  preimage 
equations — was  applied  to  the  grasp-centering  example.  Next  the  push-forward 
techniques  were  introduced  as  a  computational,  although  less  powerful  approach 
to  the  synthesis  of  multi-step  strategies.  Push-forward  algorithms  were  described, 
and  we  saw  how  LIMITED  used  these  techniques  to  generate  a  two-step  plan  for 
the  peg-in-hole  problem  with  model  error.  Finally,  we  discussed  the  limitations  of 
the  push-forward  techniques.  We  saw  that  they  were  not  powerful  enough  to  solve 
the  grasp-center  problem  in  its  entirety.  By  describing  an  experiment  where  push- 
forwards  suffice,  and  showing  an  example  where  they  are  insufficiently  general,  we 
have  tried  to  give  an  intuitive  but  fairly  precise  characterization  for  the  relative 
power  of  push- forwards. 


Fig.  7.  Strong  backprojection  of  goal  under  motion  0i,  (At  resolution  R3). 
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Fig.  10.  Forward  projection  of  start  region  under  motion  Obstacles  not 
shown.  The  arrow  shows  an  edge  of  the  forward  projection  that  resM:hed  the 
goal.  (At  resolution  R3). 
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Fig.  14.  Hq  EDR  region  for  .  (Forward  projection  minus  weak  backprojection). 
(At  resolution  R3). 


Fig.  15.  Hq  EDR  region  for  9\  (shown  without  fill  to  illustrate  degenerate  edges 
sliding  into  goal).  (At  resolution  R3). 
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Fig.  22.  Weak  backprojection  of  the  goal  under  motion  92-  (At  resolution  R3). 


Fig.  27.  Hq  EDR  region  for  dj.  (Forward  projection  minus  weak  backprojection). 
(At  resolution  R3). 
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Fig.  28.  Hq  EDR  region  for  02  (shown  without  fill  to  illustrate  degenerate  edges). 
(At  resolution  R3). 
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Fig.  32.  In  figs.  32-40  we  examine  the  commanded  motion  $2  in  detail  for  one 
slice.  The  slice  occurs  at  resolution  R3,  for  a  w  (.9,  .34).  Fig.  32  shows  the  start 
region  in  this  slice,  which  is  the  push-forward  of  motion  6\.  (Actually,  this  start 
region  should  include  the  entire  left  edge  of  the  bounding  box-  A  portion  of  the 
edge  was  not  found  due  to  a  numerical  error). 


Fig.  35.  Forward  projection  under  $2  of  the  push-forward  of  ^i.  (Detail  of 
motion  O2  in  slice  a  Ai  (.9,  .34)). 


Fig.  37.  Weak  minus  strong  backprojection  under  (Detail  of  motion  9^  in 
slice  a  «  (.9,  .34)). 


44.  Strong  backprojection  of  goal  under  motion  (At  resolution  R4). 
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Fig.  50.  Sticking  in  the  weak  minus  strong  backprojection  for  ^i.  {H, 
(At  resolution  R4). 
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Fig.  51.  Hq  EDR  region  for  9\.  (Forward  projection  minus  weak  backprojection). 
(At  resolution  R4). 
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Fig.  53.  Hq  EDR  region  for  6\,  shown  amidst  obstacles.  (At  resolution  R4). 
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Fig.  57.  Weak  backprojection  of  the  goal  under  motion  62.  (At  resolution  R4). 


Fig.  58.  W«*k  backprojection  of  goal  under  motion  Obstacles  not  shown. 
(At  resolution  R4). 
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Fig.  63.  Ho  EDR  region  for  ^3.  (Forward  projection  minus  weak  backprojection). 
(At  resolution  R4). 
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Fig.  64.  Ho  EDR  region  for  ^2,  shown  amidst  obstacles.  (At  resolution  R4). 
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11.  Failure  Mode  Analysis 


Piish-forwsurd  techniques  require  a  precise  geometrical  characterization  of  the 
forward-projection,  and  algorithms  for  computing  it.  The  gear-meshing  example  of 
chapter  I  is  a  problem  in  a  four-dimensional  generalized  configuration  space  with 
pushing.  Two  of  the  dimensions  are  rotational:  one  of  these  can  be  commanded,  and 
the  other  cannot,  but  the  position  along  this  dimension  may  be  changed  via  pushing. 
It  is  difficult  to  develop  good  forward  projection  algorithms  in  this  generalized 
configuration  space,  although  our  critical-slice  methods  are  a  start.  For  this  reason, 
a  different  technique  was  developed  for  planning  multi-step  strategies  in  this  domain. 
It  is  apphcable  for  any  generalized  configuration  space  with  the  same  degrees  of 
freedom  and  pushing  characteristics  (that  is,  any  polygonal  shapes  in  place  of  the 
gears).  The  new  technique  is  called  failure  mode  analysis;  we  describe  it  in  this 
section. 

Failure-mode  analysis  is  a  method  for  synthesizing  miilti-step  strategies  using 
a  kind  of  “approximate”  or  “a  priori”  forward  projection.  At  first  glance,  it  may 
appear  unrelated  to  push-forward  or  preimage  techniques.  However,  in  the  next 
section,  on  the  weak  EDR  theory,  we  present  a  viewpoint  which  essentially  “unifies” 
the  three  approaches. 

11.1.  Example:  Multi-Step  Strategy  for  Gear  Meshing 

Recall  the  gear-meshing  plan  LIMITED  generated  in  chapter  I,  fig.  4.  Consider 
the  problem  of  meshing  two  planar  gears,  imder  uncertainty  as  in  chapters  I  and 
II.  Suppose  that  gear  B  can  rotate  paissively  but  has  unknown  initial  orientation, 
as  above.  Suppose  that  A  has  been  gripped  by  a  robot.  The  initial  position  of  A  is 
imcertain.  The  robot  can  impart  either  pure  forces  (translations),  or  pure  torques 
(rotations)  to  A.  The  planner  can  choose  the  direction  of  translation  or  rotation. 
Can  a  multi-step  strategy  of  commanded  translations  and  rotations  be  found  to 
mesh  the  gears? 

Limited  was  able  to  generate  an  EDR  strategy  for  this  problem.  The  clmrac- 
teristics  of  the  experiment  are: 

1.  There  are  three  degrees  of  motion  freedom  (two  translational  and  one  rota¬ 
tional)  for  A. 

2.  There  is  one  degree  of  rotational  model  error  freedom  (the  orientation  of  B). 

3.  Pushing  is  possible  to  change  the  orientation  of  B. 

4.  There  is  sensing  and  control  imcertainty. 

5.  The  geometry  of  the  gears  is  complicated. 
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6.  Quasi-static  analysis  is  used  to  model  the  physics  of  interaction  between  the 
gears. 

7.  We  suppose  that  vision  is  poor,  or  that  the  gears  are  accessible  to  the  robot 
gripper,  but  not  to  the  camera.  This  means  that  position  sensing  will  be  very 
inaccurate,  and  hence  may  be  of  no  use  to  determine  whether  the  gesirs  are  suc¬ 
cessfully  meshed.  This  will  often  be  the  case  in  practice.  In  this  case,  force  sensing 
must  be  used  to  disambiguate  the  success  of  the  first  motion  (meshing)  &om  failure 
(jamming  in  an  unmeshed  state). 

8.  Hence,  a  multi-step  strategy  is  required. 

Thus  we  have  a  kind  of  four-degree  of  freedom  planning  problem  with  imcer- 
tainty  and  pushing.  To  generate  multi-step  EDR  strategies  imder  pushing,  LIMITED 
uses  the  EDR  theory  together  with  failure  mode  analysis.  Here  is  the  plan  LIMITED 
generates; 

^1.  Command  a  pure  translation  of  A  into  B.^ 

Terminate  the  motion  based  on  force-sensing  when  sticking  occurs  (when  there 
is  no  motion). 

d^.  Command  a  pure  rotation  of  A. 

If  breaking  contact  or  sticking  occurs,  signal  failure.  Otherwise,  signal  success. 

In  this  plan,  motion  9i  does  not  terminate  distinguishably  in  success  (meshed) 
or  failure  (jammed).  That  is,  after  motion  6i  terminates,  the  plan  executive  cannot 
necessarily  recognize  whether  or  not  the  gears  are  meshed.  LIMITED  predicts  this, 
and  generates  motion  62,  which  disambiguates  the  residt  of  motion  9i.  The  genera¬ 
tion  of  the  second,  disambiguating  motion  involves  the  use  of  failure  mode  analysis. 
Breaking  contact  and  sticking  are  examples  of  failure  modes.  The  second  motion  is 
generated  so  that  from  any  xinmeshed  state  resulting  from  motion  9\,  all  possible 
paths  will  terminate  distinguishably  in  a  failure  mode.  Failure  mode  analysis  is  a 
robust  subtheory  of  EDR  by  which  LIMITED  generates  multi-step  strategies  tmder 
pushing. 

11.2.  Introduction  to  Failure  Mode  Analysis 

In  the  gear-meshing  plan,  motion  ^3  is  used  to  disambiguate  the  resxdt  of  motion 
9x.  The  technique  used  is  failure  mode  analysis.  LIMITED  is  given  a  repertory  of 
qualitative  failure  modes,  which  comprise  sticking  and  breaking  contact.  Motion 
9i  can  end  in  a  “good”  region  (meshed)  or  a  “bad”  region  (jam).  LIMITED  tried 
to  generate  a  disambiguating  motion  as  a  second  step.  This  motion  is  required  to 
terminate  in  a  failure  mode  from  all  “bad”  regions. 

*  LIMITED  generates  the  actual  force  vector  Vg^ . 
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Here  is  how  LIMITED  generates  motion  Q-i-  Let  n  be  the  EDR  region  for 
motion  9i .  The  plamner  determines  all  configurations  where  motion  6\  can  terminate 
outside  of  G.  Call  this  region  pushjj(H).  pushflj(ff)  then  forms  the  start  region  for 
motion  62.  LIMITED  then  uses  quasi-static  analysis  to  “prove”  that  when  A  is  at  any 
configuration  in  push^^  (if),  and  a  pure  rotation  of  A  is  commanded,  that  all  possible 
motions  of  A  result  in  sticking  or  breaking  contact.  Sticking  and  breaking  contact 
are  called  failure  modes;  there  is  a  class  of  EDR  plans  which  cam  be  terminated  in 
failure  when  sticking  or  breaking  contact  are  detected.  EDR  planning  with  fEulure 
modes  constitutes  a  robust  subtheory  of  EDR.  It  is  a  subtheory  because  assuming 
this  kind  of  failure  mode  is  a  restrictive  assumption  to  make  planning  tractable. 
It  is  robust  because  sticking  and  breaking  contact  are  easy  to  recognize,  relatively 
speaking,  as  failure  modes  by  a  rim- time  robot  executor. 

From  the  preimage  point  of  view,  failure  modes  are  implemented  simply  as 
different  classes  of  termination  predicates. 

11.3.  Specifying  the  Goal:  Functional  Descriptions 

Recsdl  our  discussion  of  sticking  as  a  termination  condition  in  chapter  II.  Stick¬ 
ing  had  the  advantage  of  ensuring  “good”  behavior  in  the  EDR  region  H.  In 
particular,  it  could  be  guaranteed  that  all  motions  would  eventually  terminate  in 
GUH,  rendering  the  distinguishability  of  G  vs.  H  &  history-free  decision.  However, 
in  order  for  a  sticking  termination  predicate  to  generate  good  EDR  plans,  it  was  in 
fact  necessary  to  ensure  that  the  motion  strategy  has  “good”  behavior  at  the  goal 
as  well.  In  particulsu’,  the  commanded  motion  should  stick  at  the  goal. 

In  failure  mode  antdysis,  we  have  a  simil2ir  situation.  The  purpose  of  motion  $2 
is  to  force  all  motions  starting  from  pushjj(^f)  to  terminate  in  sticking  or  breaking 
contact.  Clearly  this  is  only  useful  if  no  motion  from  push^j(G)  can  even  possibly 
terminate  in  sticking  or  breaking  conteict.  This  is  the  required  “good”  behavior  at 
the  goal.  Thus,  in  an  EDR  plan  generated  by  failure  mode  analysis, 

FI.  Under  motion  82,  all  motions  starting  from  push^j(^f)  must  terminate  in  a 

failure  mode. 

F2.  No  motion  from  pushjj(G)  can  possibly  terminate  in  a  failure  mode. 

F3.  The  goal  is  a  fixed-point  under  motion  62- 

Limited  decides  whether  or  not  (Fl)  is  true.  However,  (F2)  is  given  as  input 
to  Limited.  We  will  now  discuss  how  (F2)  is  specified.  In  the  next  section  we 
will  describe  algorithms  for  computing  (Fl).  (F3)  may  be  decided  using  forward 
projections;  the  actual  condition  we  require  is 

Fg^ipxishs^iG))  C  G, 


rv.*'. 


which  is  implied  by  the  fixed-point  equation 


Fe,{G)  =  G,  (F3) 

since  of  course  push^j(G)  is  contained  in  G. 

The  goal  state  for  gear  meshing  may  be  viewed  purely  geometrically.  That  is, 
it  may  be  viewed  sis  a  set  in  genersdized  configuration  space.  This  view  is  useful 
for  computing  the  EDR  regions.  Alternatively,  the  gosJ  may  be  specified  through 
a  functional  description.  For  example,  we  might  specify  the  goal  sis  a  difference 
equation  (DE).  The  intvution  behind  this  difference  equation  formulation  of  the  gosil 
is,  "7n  iht  goal,  any  finite  rotation  of  A  results  in  an  equal  and  opposite  rotation  of 
B.  ”  More  precisely,  the  difference  equation  specifies: 

DE.  Command  any  non-zero  finite  rotation  Aoi  to  A.  In  the  goal,  this  results  in  a 

finite  rotation  of  A  by  Aoi  and  of  B  by  —  Aoi.^ 

This  difference  equation  captures  the  functional  aspects  of  the  gesus  in  their 
meshed  state.  Now,  it  is  clear  that  this  equation  may  be  “differentisdized.”  That 
is,  we  consider  it  to  be  true  for  all  non-zero  displsu:ements,  no  matter  how  smalL 
If  this  is  the  csise,  then  it  is  clear  that  breaking  contsict  is  in  direct  contiadiction 
to  the  truth  of  the  difference  equation  (DE).  This  is  because  if  contact  is  broken, 
then  there  exists  some  finite  rotation  of  A  that  will  not  affect  the  orientation  of  B. 
Similarly,  sticking  contradicts  the  truth  of  the  difference  equation,  for  if  the  gears 
stick,  then  they  are  not  properly  meshed,  i.e.,  we  do  not  obtain  equal  and  opposite 
rotations. 

In  Limited  failure  mode  analysis,  we  view  the  goal  state  as  a  combined  ge¬ 
ometrical  and  functional  specification.  Here  £ire  the  three  ways  of  specifying  the 
functional  aspects  of  the  goad.  The  laist,  which  decides  questions  about  goad  predi¬ 
cates  via  the  theory  of  read  closed  fields,  is  only  of  theoretical  interest.  The  second 
is  a  heuristic  approximation  to  such  an  inference  engine.  The  first  is  a  more  robust 
solution  with  am  engineering  flavor.  It  places  on  the  user  the  burden  of  ensuring 
well-behaved  qualitative  behavior  at  the  goal. 

11.3.1  Specifying  the  Functional  Aspects  of  the  Goal 

Method  1.  User  input.  In  this  method,  it  is  the  responsibility  of  the  user  to 
ensure  that  (F2)  is  true.  That  is,  the  user  must  gueurantee  that  failure  modes  cannot 
occur  at  the  goal.  This,  of  course,  is  the  easiest  method.  If  the  user  guarantees  that 
(F2)  holds,  then  it  remains  only  for  LIMITED  to  show  (Fl). 

^  A  and  B  are  the  same  size.  Clearly,  this  may  be  generalized  to  different  pitch  gears. 
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*11.3.2  Computational  Methods  for  Functional  Goal  Specification 

Method  2.  Inference.  If  the  user  cannot  guarantee  that  (F2)  holds,  it  is  possible 
for  Limited  to  make  certain  kinds  of  deductions  to  infer  that  (F2)  is  true. 

How  can  such  an  inference  mechanism  work?  We  can  view  the  difference  equa¬ 
tion  as  a  kind  of  predicate  on  paths.  This  is  similar  to  the  termination  predicate 
with  continuous  history  studied  by  [Mason].  In  this  model,  when  the  predicate  is 
true,  the  path  has  been  recognized  as  a  member  of  a  particulzu-  class — say,  the  goal 
class,  or  the  failure  class.  Similarly,  sticking  Eind  breaking  contact  can  be  repre¬ 
sented  as  path  predicates.  If  p  is  a  path  in  generalized  configuration  speice,  we  wish 
to  prove  that  if  the  difference  equation  predicate  (DE)  is  true  of  p,  then 

3tick{p)  V  break{p) 

is  false.  It  is  possible  to  write  a  semi-decision  procedure  for  this  question  using 
resolution  refutation.  I  wrote  a  front-end  to  LIMITED  which  can  decide  this  question 
in  special  cases,  goal,  stick  and  break  can  be  defined  as  predicates  on  paths.  To 
do  this,  we  must  view  paths  as  lying  in  phase  space,  that  is, 

p:[0,^).-^Tg. 

hence  p(t)  is  a  pair  representing  the  actual  position  of  the  robot  and  the  actual  net 
force  on  (eqmvalently,  velocity  of)  the  robot  at  time  t.  stick(p)  is  defined  to  be  true 
if  sticking  occurs  along  p.  break(p)  is  true  if  p  ever  breaks  contact. 

The  inference  system  tries  to  find  a  contradiction  ^unong  the  set  of  formulEis 

{  goal{p),3tick{p)  V  break(p)  }. 

If  a  contradiction  is  found,  the  system  assumes  that  sticking  or  breaking  contact 
cannot  occur  in  the  goal,  and  (F2)  has  been  established. 

The  quantified  difference  equation  inference  mechanism  was  implemented  to 
explore  the  feasibility  of  the  approach.  It  is  ad  hoc,  special-case,  and  incomplete.  It 
should  not  be  viewed  as  a  focus  of  this  research,  but  more  as  an  heuristic  experiment 
on  the  interaction  of  geometrical  and  functional  goal  specifications.  While  it  is 
possible  to  write  a  more  complete  inference  engine,  that  is  not  the  point  of  this 
work.  From  a  practical  standpoint,  the  user  input  method  for  ensuring  the  validity 
of  (F2)  is  probably  preferable. 

Method  3.  Second  Order  Theory  of  Real  Closed  Fields.  Method  2  described  a 
heuristic  implementation  of  a  mechanism  for  inferring  (F2)  from  a  goal  predicate. 
We  must  now  mention  a  complete,  albeit  strictly  theoretical  mechanism  for  this 
inference.  In  particular,  we  describe  a  semi-decision  procedure  for  deriving  (F2) 
from  a  goal  predicate  on  semi-algebraic  paths.  First,  we  define  an  extension  to  the 


theory  of  real  closed  fields.  Next,  we  show  it  is  semi-decidable.  Finally,  we  note 
that  the  specification  of  the  goal  predicate,  above,  may  be  encoded  in  this  language. 


Definition.  A  semi- algebraic  (s.a.)  function  is  a  univariate  piecewise-polynomial 
function. 

Definition.  The  Second  Order  Theory  of  Real  Closed  Fields  (2RCF)  is  the  first 
order  language  with  the  following  augmentations: 
quantification  over  s.a.  functions, 
s.a.  function  application, 
differentiation  of  s.a.  functions. 

While  the  first  order  theory  can  quantify  only  over  variables,  sentences  in  2RCF 
can  universally  and  existentially  quantify  over  fimctions,  such  as  s.a.  paths. 

Definition.  The  Existential  Second  Order  Theory  of  Real  Closed  Fields  (X2RCF) 
consists  of  all  2RCF  sentences  of  the  form  (3pi, . . .  ,pk  €  3i[<])F(p)  where  F(-)  is  a 
2RCF  predicate  containing  no  quantified  fimctions. 

Theorem.  X2RCF  is  recursively  enumerable. 

Proof.  Given  a  formula  (3pF(p))  where  p  G  3J[ii , . . . ,  in],  enumerate  s.a.  multino¬ 
mials  p  by  degree.  Test  whether  F(p)  is  true  using  the  first  order  theory.  □ 

Oiu:  paths  lie  in  real  d-space  where  d  is  the  dimension  of 
Now,  the  questions  we  wish  to  decide  are 

(Vp  €  (3J[t])‘^)yoa/(p)  =>  im  TT  0  p  C  dCO 
or 


(yp  €  (3?[*])‘^)poa/(p)  =>  ~'stick{p). 

The  negations  of  both  formulae  are  semi-decidable  in  X2RCF.  If  either  negation 
is  true,  then  failure  mode  analysis  will  not  work  for  this  goal  predicate.  This  gives 
a  theoretical  means  to  decide  when  failure  mode  analysis  is  inapplicable.  It  is 
interesting  only  as  an  in-principle  approach.  It  can  be  shown  that  X2RCF  is  at 
least  non-elementary.  It  is  probably  undecidable. 

Some  of  the  greatest  md  most  interesting  unsolved  problems  in  geometrical 
robotics  he  in  the  interaction  of  functional  and  geometrical  descriptions  of  goals.  In 
particular,  we  would  like  to  devise  algorithms  for  computing  a  geometrical  goal  re¬ 
gion  given  a  functional  description — for  example,  a  quantified  difference  equation — 
for  the  desired  behavior  in  the  goal  state.  Conversely,  we  would  like  to  be  able  to 

^Actually,  d  is  the  dimension  of  a  real  space  in  which  the  manifold  TQ  embeds. 
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infer  a  functional  description  of  the  goal  from  its  geometrical  aspects.  The  latter 
would  be  useful  in  automatically  generating  termination  predicates  to  recognize  the 
goal. 

11.4.  Approximate  Algorithms  for  Failure  Mode  Analysis 

We  now  describe  algorithms  for  deciding  whether 

Fl.  Under  motion  62,  all  motions  starting  from  pushgj(if)  must  terminate  in  a 

failure  mode. 

Let  us  denote  push^^(if)  by  H\.  These  algorithms  use  time-indexed  forw^lrd 
projections  to  prove  that  imder  62,  all  paths  starting  in  H\  eventually  stick  or  break 
contact.  The  algorithms  are  approximate,  although  conservative.  That  is,  ii  they 
terminate  then  (Fl)  is  true.  However,  they  may  not  terminate  if  (Fl)  is  false,  and 
they  may  miss  cases  where  (Fl)  is  true.  The  accuracy  of  the  algorithm  increases 
as  the  time  steps  for  the  time-indexed  forward  projections  are  taken  to  be  finer.  In 
the  4D  generalized  configuration  space  for  the  gears,  which  is  x  x  5^,  these 
time-steps  correspond  to  the  fineness  of  the  slice  resolution  across  the  rotational 
dimensions. 

We  will  first  describe  a  quite  general  algorithm  for  deciding  (Fl).  It  is  ap¬ 
plicable  wherever  we  can  obtain  a  computational  chareicterization  of  time-indexed 
forward  projections.  Later,  we  will  give  a  specialized  algorithm  in  the  generalized 
configuration  space  for  the  gears,  and  show  that  it  is  in  fact  a  special  czise  of  the 
general  algorithm. 

11.4.1  A  General  Algorithm 

The  basic  idea  is  to  step  along  in  time,  simulating  the  motion,  and  determine 
whether  or  not  it  breaks  contact  or  sticks.  Of  course,  we  must  simulate  all  possible 
motions,  using  forward  projections. 

First  we  must  develop  some  notation.  Recall  that  for  a  planar  set  Hi,  dHi 
denotes  its  obstacle  edges.  Here,  we  will  use  it  to  more  generally  to  denote  the 
obstacle  surfaces  (as  opposed  to  the  free-space  surfaces)  boimding  a  set  H)  in  gen¬ 
eralized  configuration  space.  (In  our  case  H\,  the  input  to  the  algoritliiii,  is  the 
push-forward  of  motion  ^1). 

Let  X  be  a  point  in  generalized  configuration  space.  Then  stick0{x)  is  true  if 
sticking  is  necessary  at  x  under  all  control  velocities  Becivg)  consistent  with  the 
nominal  commanded  velocity  Vg.  Let  stickg(Hi)  denote  all  points  x  in  Hi  where 
stickff(x)  holds. 


Now,  assiime  some  positive  minimum  modulus  bound  on  the  commanded  ve¬ 
locity.  We  use  Fs,a<(‘)  as  the  time-indexed  forward  projection  operator  (see  [Erd- 
maim]).  So  denotes  the  set  of  possible  positions  the  robot  can  be  at  at 

time  At,  having  started  in  Hi  at  time  t  =  0. 

Now,  we  are  ready  to  give  the  general  algorithm  for  deciding  (FI): 

Algorithm  Gen 


1.  Let  F  *—  Fo^^t{Hi). 

2.  Let  H2  *—  dF  —  3ticke^{dF)  . 

3.  When  H2  =  0,  we  have  proven  that  all  paths  from  Hi  must  eventually  stick  or 
break  contact.  Halt. 

4.  Else,  Hi  *—  H2.  Goto  (1). 

Note  that  Hi  is  permitted  to  be  in  free-space,  although  given  the  sticking  push- 
forward  it  will,  in  iojct,  always  be  on  a  generalized  configuration  space  boundary. 
Note  that  (7en  is  a  semi-decision  procedure.  Clearly,  if  the  algorithm  halts,  then 
all  paths  originating  in  Hi  eventually  break  contact  or  stick.  Fig.  67  illustrates  the 
algorithm.  Suppose  the  Hi  region  is  the  edge  e.  Its  forward  projection  after  At 
is  the  region  U  g.  The  obstacle  edges  of  the  forward  projection  are  c',  /,  and  g. 
Sticking  must  occur  on  /.  Hence,  H2  is  e’  U  g. 

We  now  mention  a  basic  property  of  forward  projections  that  this  algorithm 
exploits.  It  is  the  property  that  forward  projection  commutes  with  union.  In 
particular,  if  we  have 


boundary  free— »p»ce 

=  'S?  -H  . 


then 


FsiHi)  =  FeiHg  UHf)  =  FsiHg)  U  FeiHp). 

This  key  property  permits  the  algorithm  to  decompose  the  failure  mode  amalysis 
into  essentially  independent  decision  problems  about  the  forward  projections  of  the 
free-space,  sliding,  and  sticking  regions  in  the  push-forward. 

11.4.2  A  Specialized  Algorithm 


For  failure-mode  analysis,  LIMITED  employs  an  algorithm  that  is  a  sp^rinl  rase 
of  the  general  algorithm  above.  The  idea  is  that  when  commanding  a  puK  h  >fation 
of  A,  the  time-indexed  forward  projection  across  slices  can  be  well  approxiiu;itrd  by 


Fig.  67.  Dlustration  of  the  general  algorithm.  The  start  region  H\  is  the  edge 
e.  Its  forward  projection  after  At  is  the  region  U  g.  The  obstacle  edges  of 
the  forward  projection  are  e',  /,  and  g.  Sticking  must  occur  on  /.  Hence,  is 
e'  U  g. 


the  differential  forward  projection  of  sec.  6.  The  differential  forward  projection  is  a 
technique  for  propagating  the  forward  projection  across  slices,  when  rotations  of  A 
and  B  axe  permitted.  Recall  our  notation  for  motions  ffi  and  ^3.  is  a  commanded 
pure  translation  of  A,  and  may  be  viewed  as  unit  vector  in  the  plane.  ^3  is  a 
commanded  pure  rotation  of  A,  and  may  be  viewed  as  a  member  of  {  +<ioi ,  — dai  }, 
for  positive  and  negative  commanded  rotations. 


Difftrtniial  and  Propagated  Forward  Projections 


Pure  Translations.  Forward  projections  mxist  be  propagated  between  slices 
even  when  a  pure  translation  is  commanded,  since  a  pure  translation  9\  can  alter 
the  orientation  of  B,  and  hence  the  slice-value,  through  piishing.  Recall  Ii<  >\v'  the 
differential  forward  projection  is  constructed  for  a  pure  translation  9i  f«'».  6). 
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A.  ^  if j  v  v-  m'.vIvv..'-  »: .  a-V a'-  »•-  a.  ji j  a*.  .Cmj 


Let  (x, y, ai,a2)  denote  a  configuration  in  the  generalized  configuration  space  for 
the  gears,  x  S*  x  S^.  (x,j/,ai)  denotes  a  configuration  of  A.  02  denotes  the 
configuration  of  B.  Hence,  we  regard  the  orientation  of  B  (the  “last”  S*  in  the 
product)  as  J.  Now,  is  a  set  in  generalized  configuration  space.  Let  .Hilai.oj 
denote  a  psurticular  x-y  slice  of  Hi  for  orientation  oi  of  A  amd  02  of  B. 

Motion  $1  commands  a  pure  translation  of  A.  Now,  for  each  edge  in 
Limited  performs  a  quasi-static  analysis  to  determine  the  possible  impending  mo¬ 
tions  of  A  and  B.  That  is,  it  determines  which  way(s)  A  and  B  can  rotate.  These 
directions  may  be  viewed  as  tangent  vectors  to  the  pure  rotational  dimensions  of 
generalized  configuration  space.  The  set  of  possible  directions  may  be  identified 
with  a  set  of  pairs 

{  —dai ,  0,  -l-dai  }  x  {  -daj ,  0,  +da2  }  (9) 

in  the  tangent  space  to  (5^  x  5^).  By  performing  this  smalysis  for  all  edges,  we 
obtain  a  set  of  directions, 

dJtfj  (/fi  |o,j  )i 

which  is  called  the  differential  forward  projection  of  Hi  l^j  under  di.  It  is  assumed 
that  commanding  di  from  region  c&n  result  in  any  motion  direction  in  this 

set. 

Suppose  (0^,02)  is  a  slice  taken  in  the  direction  of  some  tangent  vector  v  in  the 
differential  forward  projection.  For  example,  if  v  =  {+dai ,  —doi2),  then  a'j  =  ai  -f  ci 
and  02  =  02  —  «2  for  some  small  positive  scalars  ei  and  62' 

Now,  the  forward  projection  may  be  propagated  to  the  adjacent  slice  (01,02) 
as  follows.  An  edge  Cj  in  Hi\ai,at  corresponds  to  the  intersection  of  an  edgebraic 
surface  V  in  generalized  configuration  space  with  the  “plzme”  3?^  x  {  (oi,  ©2)  }.  V  is 
followed  into  (oj ,  O2 ),  and  the  forwaurd  projection  of  is  taken  to  be  the  intersection 
of  V  with  the  “plane”  3?^  x  { (0*1,02)  }.  In  this  manner,  we  obtain  a  set  of  edges 
{  e\ }  in  the  new  slice.  The  pure  trainslational  forward  projection  of  these  edges 
\mder  81  is  then  computed  within  this  slice,  so  the  propagated  forward  projection  is 
Fg^{{e^  }).  This  propagated  forward  projection  is  computed  at  a  fixed  orientation 
of  A  aind  B.  Ideally,  the  planner  should  decide  whether  the  sliding  characteristics 
change  along  V  while  moving  through  rotation  space.  The  rotational  values  which 
are  sliding-critical  are  discussed  in  the  critical  slice  section,  6.3.^  The  propagated 
forward  projection  increases  in  accuracy  as  the  slices  are  taken  closer  together. 

Pure  Rotations.  Consider  the  problem  of  computing  forward  projections  2w:ross 
slices  for  a  commanded  pure  rotation  82  €  {  -\-doi,  —doi  }.  For  simphcity,  we  first 

^Detecting  sliding  critical  orientation  parameters  along  the  algebraic  surface  V  has  not 
been  implemented  in  LIMITED.  Thus  the  propagated  forward  projection  may  be  larger 
than  it  need  be. 

203 


consider  the  case  where  JJi  consists  of  a  single  point.  Let  x  be  a  point  in  the 
plane,  and  (x,  01,03)  be  a  configuration  where  A  and  B  are  in  contact.  Then  the 
differential  forward  projection  of  x  under  62  will  consist  of  vectors  in  the  set  of 
eq.  (9).  The  differentied  forward  projection  has  the  same  structure  as  in  the  pure 
translational  case.  It  may  be  computed  using  quasi-static  analysis,  (see  the  next 
subsection  below). 

Suppose  for  the  sake  of  development  that  the  differential  forwjuxi  projection 
consists  of  exactly  one  direction  v,  and  that  (01,0:2)  is  an  adjacent  slice  in  that 
direction,  as  above.  Now  we  ask,  what  is  the  propagated  forwrurd  projection  of  x 
into  the  adjacent  slice,  (aJjOj)?  Well,  it  can  be  one  of  two  things:  either  it  is  x, 
or  it  is  empty.  The  reason  is  that  x-y  position  is  invariant®  imder  62.  Thus,  an 
upper  botmd  on  the  propagated  forward  projection  of  into  an  adjacent 

slice  (01,0:2)  is  found  by  simply  “copying”®  into  slice  (01,02). 

Now,  consider  the  propagated  forward  projection  of  (x,  01,02),  tmder  motion 
^2,  into  shce  01,03.  It  is  simply  the  point  (x,  01,03).  There  are  three  possible 
qualitative  outcomes: 

1.  X  is  inside  a  generalized  configuration  space  obstacle  in  slice  (01,03). 

2.  X  is  in  free  space  in  slice  (ol, 03). 

3.  X  is  on  the  boimdary  of  a  generalized  configuration  space  obstacle  in  slice 

(ol,o^). 

Obviously,  (2)  implies  that  contact  has  been  broken.  (1)  corresponds  to  a 
physically  impossible  sitviation.  Since  the  configuration  (x, o^jOj)  is  physically 
unattainable,  this  means  that  the  commanded  motion  $2  must  result  in  sticking 
(no  actual  motion)  before  (01,03)  can  be  reeiched.  Now,  if  we  have  either  outcome 
(1)  or  (2)  then  we  have  proven  that,  imder  ^3,  any  path  for  the  robot  starting  at 
(x, 01,02)  must  stick  (1)  or  break  contact  (2). 

Suppose,  however,  we  have  outcome  (3).  This  outcome  is  not  inconsistent 
with  the  negation  of  (Fl).  That  is,  it  has  not  yet  been  shown  that  any  path  from 
(x, 01,03)  will  stick  or  break  contact.  In  this  case,  in  the  new  slice  (01,03)  we 
again  perform  the  quasi-static  analysis  and  forward  project  again  into  yet  another 
slice.  This  process  continues  until  either  outcomes  (1)  or  (2)  are  obtained. 

More  generally,  the  differential  forward  projection  of  (x,  01,03)  could  consist 
of  more  than  one  vector.  In  this  case,  each  must  be  taken  as  a  forward  projection 
direction,  and  in  each  direction  we  must  show  that  outcomes  (1)  or  (2)  eventually 
occur.  That  is,  the  computation  above  must  performed  for  each  direction  predicted 

®See  below  for  more  on  this  assumption. 

®We  use  the  awkward  term  “copying”  instead  of  “translating”,  since  while  the  latter  is 
precise  mathematically  it  is  confusing  robotically. 


by  the  quasi-static  zuialysis,  and  all  directions  must  terminate  in  sticking  or  breaking 
contact. 

We  have  described  how  the  failtire  mode  analysis  proceeds  when  the  push- 
forward  Hi  of  the  first  motion  9i  is  simply  a  point.  It  remains  to  generailize  the 
discussion  to  the  case  where  Hi  is  a  region  in  generalized  configuration  space,  rep¬ 
resented  by  slices.  We  first  introduce  some  notation.  If  CO  denotes  the  generalized 
configuration  space  obstacle  for  A  due  to  H,  then  let  CO\ai,ai  denote  the  x-y  slice 
of  CO  at  orientations  (01,02)-  As  usual,  let  d  denote  the  obstacle  edges  of  a  set. 
€  is  the  slice  resolution  parameter.  The  input  to  this  procedure  is  a  stack  Q  of 
i-y-slices  of  Hi.  An  entry  in  Q  is  a  triple,  consisting  of  2m  x-y  slice  Hi\Q^,a,,  and 
(01,02),  the  orientations  at  which  the  slice  was  computed. 

Algorithm  Spec 

1.  Do  until  Q  =  0: 

2.  Pop  the  triple  (Hi|ai,a,,0!i,02)  off  Q.  Let  H2  *-  Hi  Id, a,. 

3.  Let  dF  -  dFd^iHi). 

4.  For  each  v  m  dF  do: 

5.  Xet  (01,03)  ^  €V -I- (01,02). 

6.  Compute  CO\a>^,a‘^‘ 

7.  XeiHa  <-H2naC01o,-.ai. 

8.  If  Hz  ^  0,  push  the  triple  (H3, 01,02)  onto  Q. 

Note  that  this  is  a  semi-decision  procedure.  This  is  the  algorithm  that  is 
zu:tually  implemented  in  LIMITED.  The  key  step  is  of  course  the  iteration  step  (7), 
which  we  think  of  as 

“H2-H2naCC|„;,„f 

which  is  repeated  “until  H2  is  null.”  COla\,a'j  is  computed  using  the  plane  sweep 
union  algorithm,  as  is  the  intersection. 

11.4.3  On  the  Invariance  Assumption 

We  have  assumed  that  x-y  position  of  A  is  invariant  imder  a  commanded  pure 
rotation  O2.  That  is,  commanding  a  pure  rotation  cannot  result  in  an  induced 
translation.  On  the  other  hand,  we  allow  a  commanded  pure  translation  of  A  to 
induce  a  rotation  of  B  (but  not  of  A).  These  assumptions  are  realistic  if,  for  example, 
the  robot  has  gripped  A  by  its  center  shaft,  and  the  manipulator  is  very  stiff  in  the 
x-y  directions  when  commanding  a  pure  rotation.  In  future  work,  relaxing  this 
Eisymmetry  should  be  explored.  See  chapter  VI  for  suggestions. 
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11.4.4  Quasi-Static  Analysis 


We  now  show  how  the  quasi-static  analysis  is  computed.  It  is  quite  simple. 
We  view  the  commanded  velocity  to  A  as  w  =  (0,0,  ±1).  When  the  gears  are  in 
contact,  this  defines  a  moving  constraint  in  the  configuration  space  of  B,  which  is  a 
one-dimensional  space.  Given  a  contact  configiuation,  we  compute  the  moment  arm 
in  order  to  determine  the  direction  of  the  constraint.  The  moment  arm  on  B  (resp., 
A)  is  simply  the  vector  from  5’s  (resp.  A’s)  center  of  mass  to  the  contact  point  in 
real  space.  The  contact  point  in  real  space  can  be  recovered  from  the  contact  point 
in  configxiration  space. 

Let  To  and  rj  denote  the  moment  arms  on  A  and  B,  resp.  Then  the  instan¬ 
taneous  velocity  Va  of  the  contact  point  on  A,  given  ui,  is  uj  x  r*.  B’s  direction  of 
impending  motion  is  given  by  the  sign  of  the  expression 


n  X  ir^Va  =  rj  X  7r2(ti;  x  ra). 


where  ^2  denotes  the  projection  of  onto 

We  now  discviss  recovery  of  the  moment  arms  from  the  contact  configuration. 
Let  COMa.  and  COMb  denote  the  centers  of  mass  of  A  and  B.  In  these  exper¬ 
iments,  they  are  simply  the  centers  of  the  gears.  Suppose  (x,  01,02)  is  a  contact 
configuration.  Then  it  lies  on  an  algebraic  surface  in  the  generalized  configuration 
space  X  X  S^.  This  svirface  is  one  of  two  types  [Lozano-Perez].  Let  —A  denote 
the  reflection  of  A  about  its  reference  point.  A  type  (A)  siorface  is  generated  by  an 
edge  Co  of  —A  smd  a  vertex  bj  of  B.  A  type  (B)  surface  is  generated  by  a  vertex 
Oj  of  —A  and  an  edge  ej  of  B.  Each  edge-vertex  or  vertex-edge  pair  is  called  the 
generator  pair  of  the  constraint  surface  [Donald].  The  edges  and  vertices  of  —A 
(resp.  B)  rotate  with  oi  (resp.,  02).  An  (oi, 02)-slice  of  the  surface  is  foimd  by 
rotating  its  generators  by  (qi  ,  02)5  and  taking  their  Minkowski  sum.  Hence  the  sur¬ 
face  may  be  viewed  as  a  parameterized  line-equation,  by  (01,02).  The  table  below 
gives  the  details  for  recovering  the  moment  arms  from  the  contact  configuration, 
contact  surface  in  generalized  configuration  space,  and  centers  of  mass.  We  employ 
the  following  notation.  For  an  edge  c  or  a  vertex  v,  e(o)  and  v(o)  respectively 
denote  e  and  v  rotated  to  orientation  o.  0  denotes  convolution  (sometimes  known 
as  the  Minkowski  sum).  For  two  sets  U  and  V,U®V  =  {v  +  u\  ueU,  v  £  V  }. 


Type 

Surface 

Moment  arm  on  B 

Moment  arm  on  A 

n 

ra 

A 

ea(ai)0  6^(02) 

6,(02)  -  COMb 

bj{a2)-x-COMA 

B 

aj(ai)  ©  64(02) 

X  —  a,(oi)  —  COMb 

— a,(oi)  —  COMa 
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11.4.5  Stiction 


What  the  Spec  algorithm  does  is  this:  it  tries  to  show  that  from  any  slice  of 
Hi,  aXL  pwiths  that  could  possibly  evolve  from  commanding  a  rotation  of  A  either  (1) 
remain  in  the  first  slice,  or  (2)  in  some  subsequent  slice,  stick  or  break  contact.  We 
have  described  how  (2)  is  detected.  (1)  is  a  form  of  stiction;  the  gears  do  not  turn. 
Note  that  (1)  is  a  form  of  sticking  behavior,  since  no  motion  occurs.  Staying  in 
the  same  slice  means  that  (ax,  02)  are  fixed,  and  x  and  y  are  fixed  a  priori.  Hence 
events  (1)  or  (2)  satisfy  (Fl).  That  is,  (1)  is  also  a  form  of  sticking,  smd  can  be 
detected  at  nm-time  by  the  termination  predicate. 

Now,  suppose  B  sticks  but  A  continues  to  turn?  This  type  of  stiction  is  also 
no  problem,  since  it  corresponds  to  a  differential  motion  (±dai,0),  which  can  be 
predicted  by  the  differential  forward  projection. 

11.4.6  Failures  Outside  the  EDR  Framework 

We  will  momentarily  digress  to  a  practical  question.  It  would  appear  that 
for  failure  mode  analysis  to  work,  non-uniform  stiction  wotdd  be  required  in  our 
physical  model  of  the  gears.  That  is,  it  would  seem  that  stiction  would  have  to  be 
impossible  in  the  goal,  but  possible  in  Hi.  This  is  not  the  assumption  made  in  the 
geometrical  EDR  analysis  and  implementation.  We  now  show  that  uniform  stiction 
is  in  fact  not  an  impediment  to  failure  mode  analysis,  either. 

It  is  the  responsibility  of  the  user,  or  of  some  external  inference  system,  to 
ensure  that  (F2)  holds.  Suppose,  however,  that  this  inference  is  incorrect,  and  that 
at  nm-time  stiction  does,  in  fact,  occur  in  the  goal,  and  that  the  gears  jam.  In 
this  case  the  run-time  executive  will  signal  failure,  even  though  the  geometrical  goal 
has  been  achieved.  At  first  glance  it  appears  that  this  is  incorrect.  However,  when 
we  regard  the  goal  as  a  combined  geometrical  and  functional  specification,  it  is 
clear  that  this  is  actually  the  correct  termination  diagnosis.  That  is,  even  though 
the  geometrical  goal  hats  been  Ew:hieved,  stiction  prevents  the  quantified  difference 
equation  (DE)  on  paths,  goal{-),  from  being  satisfied.  Since  something  (specifically, 
stiction)  has  prevented  achievement  of  the  functional  goal,  it  is  completely  correct 
for  the  run-time  executive  to  signal  failure  in  this  case.  However,  note  that  we  regard 
this  as  serendipitous  failure  detection,  and  not  as  inherent  in  the  EDR  framework. 

11.4.7  Generalizations 

The  specialized  algorithm  Spec  may  be  generalized.  The  prop>erties  it  exploits 
are  (1)  that  certain  degrees  of  freedom  in  C  and  J  can  be  held  fixed,  while  others 
may  be  commanded,  (2)  that  “slices”  of  CO  can  be  computed,  (3)  set  intersections 
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can  be  computed,  and  (4)  differential  motion  across  the  non-fixed  degrees  of  freedom 
can  be  predicted  using  quasi-static  analysis. 

More  precisely,  the  specialized  algorithm  generalizes  to  cases  where  we  fix  cer¬ 
tain  degrees  of  freedom  C/  and  Jf,  command  Cc,  and  permit  Jc  to  vary  (through 
pushing).  Hence  Q  is  decomposed  into 


Cf  X  Cc  X  Jc  ^  Jf, 

Bee(vfj)  lies  in  the  tangent  space  to  Cc,  and  all  motion  lies  in  the  subspace  Cc  x  Jc. 
Using  quasi-static  emalysis,  we  predict  the  impending  motion  direction,  v  which  lies 
in  the  tangent  space  to  Cc  x  Jc.  If  a  is  in  Cc  x  Jc,  let  JEfija  denote  a  shce  of  Hi  at 
(a).  Thus  Cf  X  Jf  are  the  dimensions  of  the  slice  (like  x,  y  in  the  gear  example). 
Then  we  let  a'  a  -f-  cv.  Finally,  the  iteration  step  is 

Hi  HilaOdCOla'. 

The  rest  of  the  algorithm  goes  through  mutatis  mutandis.  This  generalization  is 
somewhat  theoretical,  in  that  in  practice  the  CO-shces,  set  intersections,  and  quasi¬ 
static  analysis  may  be  diflBcult  to  compute  for  higher-dimensional  problems. 


*  11.4.8  Discussion:  General  vs.  Specialized  Algorithm  for 
Failure-Mode  Analysis 

This  starred  subsection  may  be  skipped  at  first  reading.  It  contains  a  detailed  proof. 

The  problem  with  implementing  algorithm  Gen  directly  is  that  arbitrary  time- 
indexed  forward  projections  are  difficult  to  compute.  For  this  reason  we  introduced 
a  specialized  algorithm  for  the  gear  planning.  While  algorithms  Spec  and  Gen 
appear  quite  different,  in  fact.  Spec  is  simply  a  special  case  of  Gen.  The  motivation 
behind  this  viewpoint  is  to  find  a  uniform  framework  for  characterizing  algorithms 
for  failure  mode  analysis.  That  is,  algorithm  Gen  can  be  viewed  as  a  liigh-level 
computational  approach  to  failure  mode  analysis,  while  Spec  is  an  implementation 
of  Gen  in  a  restricted  domain.  We  now  discuss  this  view  of  the  algorithms. 

Recall  the  definition  of  sticA:#j(-).  We  now  define  stickg^{R)  to  be  all  points 
X  in  iZ  such  that  any  feasible  path  from  x  consistent  with  the  control  uncertainty 
Beciv0^),  eventually  sticks. 

We  employ  the  following  topological  notions.  W  denotes  the  closure  of  a  set  U. 
denotes  its  complement.  i{U)  denotes  its  interior.  U  denotes  the  complement 
of  the  closure. 

Now,  consider  the  following  step  of  the  Spec  algorithm, 

7.  Hi^H2r\dCOU'^,c.',, 

where  H2  =  .ffiloi.oj-  This  step  is  equivalent  to 


(10) 


Hz  *—  H2  *(C'0|o' )  ~  (^Ofo[,a'j  * 
where  the  set  difference  operator  —  associates  to  the  left.  Now,  the  set 

H2  n  iiCO\a\,a'^) 

corresponds  to  all  configxirations  (x,  01,02)  in  the  planar  slice  (01,02)  such  that 
under  02,  any  path  from  (x,  01,02)  will  stick  before  reaching  (oi.Oj)  if  x  is  kept 
fixed.  That  is,  it  is  configiurations  such  that  sticking  will  occur  from  (x,  01,02) 
between  (01,02)  and  (o'i,a2). 

Below,  we  argue  that  the  set  H2  n  i{CO\a\,a0  in  algorithm  Spec  corresponds 
in  a  quite  precise  fashion  to  stick$^(dF)  in  algorithm  Gen.  We  see  this  as  follows: 
The  following  step  of  the  Gen  algorithm,^ 

2.  Hz  <—  dF  —  stick0,{dF)  . 

is  equivalent  to 


Hz^  F-  -  sticke^idF). 
Now,  it  is  possible  to  modify  Gen  as  follows.  Let 


(11) 


F2  =  Fej,^t(Hi  -  stickg^iHi)). 

Then  we  can  replace  the  assignment  (11)  by  eq.  (12)  and  still  have  Gen  be  correct: 

Hz^F2-  (12) 

We  wish  to  compare  the  step  (12)  of  the  thus  modified  Gen  with  the  step  of 
Spec  given  in  eq.  (10).  In  essence,  we  wish  to  show  that  eq.  (10)  is  in  some  sense  a 
“conservative”  approximation  to  eq.  (12),  and  hence  conclude  that  algorithm  Spec 
is  simply  a  special  case  of  algorithm  Gen. 

We  must  introduce  some  notation  to  compare  eqs.  (10)  and  (12).  For  a  set  V 
in  ,  we  denote  the  set 


by 


V  X  {  (01,02)  } 


V  X  (01,02). 


^  We  have  lexicographically  substituted  Hz  for  H2  throughout  algorithm  Gen  to  facilitate 
the  comparison  with  Spec. 


Now,  Hi  is  a  subset  of  Q.  A  slice  of  it  /fiid.oj  lies  in  the  “plane”  3?^  x  (oi,a2). 
Let  us  denote  its  projection  into  by  Tr2Hi\ai,a,-  Finally,  for  an  arbitrary  set  U 
in  generalized  configuration  space,  let  17Ui,o,  denote  an  (oi, a2)-8lice  of  it,  that  is. 


—  U  n  (3?^  X  (ori,  Qr2)). 


Claim:  Eq.  (10)  ia  a  conservative  approximation  to  eg.  (12)  in  each  slice. 
Proof.  First,  we  obviously  have 


C  CO. 

Next,  we  need  only  show  that 


ori  ,aj  ))  C7r2(^f,|  ai,aj  )x(ai,a^) 

\  /  a',  .o,'. 


n7r2^l(COU;,o',)^  X  (01,02)  C  3tickg^{Hi\a^^a,)- 


Eqs.  (14)  and  (15)  are  definitional.  Now,  suppose  that  configuration  z  € 
t(CO|a<  Then  clearly  z  ^  F|o',o'^.  Hence  we  have 


Hz, Gen  =  (F’«,,A«(-ffl  -  Stic*:;  (/fi))^!  -  CO\a>aC 


Hz, Spec  =  ^’'■2H'2  X  (0^,02)^  —  i(CO|a<,o<^)  ”  CO\a'^^a’^ 


Note  that  as  a  consequence,  we  may  expect  that  Spec  is  less  likely  than  Gen 
to  terminate. 


M 


f 


I 


1 


I 

I 
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12.  Weak  EDR  Theory,  Strategy  Equivalence,  and  the 
Linking  Condition 

12.1.  Reachability  and  Recognizability  Diagrams 

We  now  introduce  a  type  of  diagram  which  permits  notation  of  reachability 
and  recognizability.  These  diagrams  are  a  powerful  tool  for  compactly  expressing 
motion  strategies.  They  greatly  aid  the  development  of  concise  and  readable  proofs. 

Suppose  we  are  given  a  start  region  R,  a  goal  G,  and  a  motion  9.  We  construct 
the  EDR  region  H.  Then  under  sticking  termination,  all  motions  from  R  will 
terminate  in  G  or  H.  That  is,  the  push-forward  of  the  motion  9  from  R  is  contained 
in  GU  H: 


pushj(Ftf(ii))  C  G  U 


(16) 


Whenever  (16)  is  true,  we  write  this  by  the  following  reachability  diagram. 


R 


(17) 


Suppose  that  G  and  H  are  distinguishable  using  sensors.  Then  0  is  an  EDR 
strategy  from  R,  and  we  have 


R  =  P0,Ri{G,H}).  (18) 

Whenever  (18)  holds,  we  write  this  oy  the  following  recognizability  diagram, 

G 

R  (19) 

«  H. 

The  reachabihty  diagram  (17)  is  an  equivalent  notation  for  the  reachability 
termination  condition  (16).  The  recognizability  diagram  (19)  is  equivalent  nota¬ 
tion  for  the  recognizability  termination  condition  (18).  Single  arrows  ( — ►)  denote 
reachability  whereas  double  arrows  (=^)  denote  recognizability.  If  and  only  if  (16) 
is  true,  we  say  that  the  correspondingly  reachability  diagram  (17)  holds.  If  and 
only  if  (18)  is  true,  we  say  that  the  correspondingly  recognizability  diagram  (19) 
holds.  A  diagrtim  is  said  to  hold  tautologously  when  it  is  true  without  additional 
conditions  or  suppositions. 
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The  nice  thing  about  sticking  termination,  as  discussed  in  chapter  II,  is  the 
following  property: 

Theorem:  Lei  R  be  a  start  region,  6  a  motion,  and  G  a  goal.  Construct  the  EDR 
region  H  for  R,  6,  and  G.  Then  with  sticking  termination  the  reachability  diagram 
(17)  holds  tautologously. 

Now,  in  diagrams  (17)  and  (19)  we  have  labeled  all  the  arrows.  In  the  future, 
when  this  would  clutter  the  diagrams,  we  will  label  only  the  top  arrow  and  adopt 
the  convention  that  ail  arrows  aligned  below  it  have  the  same  label. 

12.2.  More  General  Push-Forwards 

Hence  the  chief  advantage  with  sticking  termination  is  that  (17)  is  always 
true.  In  this  chapter,  we  will  generally  assume  that  either  sticking  termination  is 
employed,  or,  if  more  general  termination  predicates  are  allowed,  then  the  truth  of 
the  reachability  diagram  (17)  can  be  determined  through  restrictions  on  time  and 
history,  as  described  in  chap.  II.  We  now  digress  briefly,  however,  to  describe  how 
this  discussion  generalizes  for  more  general  termination  predicates. 

In  an  appendix,  we  define  a  more  general  push-forward,  F»g{R),  which  de¬ 
notes  all  configurations  at  which  the  motion  6  can  terminate  given  more  general 
termination  predicates.  When  more  general  termination  predicates  than  sticking 
are  considered,  then  the  condition  (16)  must  be  repleiced  by 

F,eiR)cG\JH.  (20) 

When  (20)  holds,  we  may  then  write  the  equivalent  reachability  diagram  (17). 

However,  with  more  general  termination  conditions,  (17)  does  not  hold  tau¬ 
tologously.  For  example,  with  time- termination  and  the  approximate  push- forward 
described  in  sec.  10.3,  a  motion  could  (a  priori)  terminate  without  sticking  yet 
within  the  weak  preimage.  In  such  cases,  it  must  be  the  responsibility  of  the  plan¬ 
ner  to  verify  that  all  motions  terminate  in  G  U  if. 

The  first  difference  between  the  sticking  push-forward  push(-)  and  the  general 
push-forward  F,(-)  is  that  F,(-)  depends  on  the  start  region  for  the  motion,  while 
push(-)  does  not.  That  is,  F,(  )  depends  on  history  (and  possibly  time)  whereas 
pirsh(-)  does  not. 

Now,  a  motion  sequence  is  a  reachability  or  recognizability  diagram  of  the  form; 

•  ■  •  Jl^R,-^Rc  =  G.  (21) 

The  second  chief  difference  between  the  a  priori  sticking  push-forward  push(-) 
and  the  general  push- forward  F,(-)  is  that  the  action  of  push(-)  on  a  motion  sequence 


(21)  is  funciorial,  while  i^.(  )  is  not.  The  non-fiinctoriality  of  is  a  consequence 
of  its  history  dependence. 


12.3.  Weak  EDR  Theory 

We  now  make  the  following  natural  refinement  of  our  termination  predicate. 
Suppose  the  termination  predicate  is  given  some  finite  collection  of  goals  {  }  in 

a  distinguishable  union.  Then  the  goals  {G/j}  eire  of  course  partially  ordered  by 
containment.  We  assume  that  the  termination  predicate  returns  the  smallest  goal 
(with  respect  to  containment)  if  at  termination  time  the  actual  configuration  of  the 
robot  is  known  to  lie  within  two  or  more  goals.  (A  technical  point:  if  two  or  more 
goals  overlap,  we  augment  the  collection  with  a  new  goal  which  is  their  intersection). 

Now,  whenever  the  reachability  diagram  (17)  holds  (which  it  always  does  with 
sticking  termination),  then  we  have  the  following: 

R  =  Pe,R{{G^H,G\jH)).  (22) 

This  is  trivial  to  show;  on  termination,  the  termination  predicate  will  return  G  or 
H  if  it  can,  otherwise  it  will  return  G  U  H.  In  particular,  it  will  return  G  or  H  in 
preference  to  G  U  H. 

Thus  we  can  write  the  following  recognizability  diagram,  which  is  equivalent 
to  (22): 

G 

R  =U  H  (23) 

9  GtJH. 

(23)  is  called  the  Weak  EDR  Recognizability  Diagram  for  G,  H ,  and  9.  (19) 
is  called  the  Strong  EDR  Recognizability  Diagram.  (17)  is  called  the  Reachability 
Diagram. 

Theorem:  Let  R  be  a  start  region,  9  a  motion,  and  G  a  goal.  Construct  the  EDR 
region  H  for  R,  9,  and  G.  Then  with  sticking  termination  the  weak  EDR  diagram 
(2S)  holds  tautologously. 

Up  to  now,  in  previous  chapters,  we  have  described  the  strong  EDR  theory. 
This  section  has  introduced  the  weak  EDR  theory.  It  may  not  appear  useful  at 
first  glance.  However,  in  the  next  section  we  will  see  that  these  one-step  weak  EDR 
strategies — which  are  in  effect  always  a\’ailable — may  under  certain  conditions  be 
chained  together  to  make  a  multi-step  plan  very  like  a  strong  EDR  strategy. 


The  key  idea  behind  the  weak  EDR  theory  is:  given  a  collection  of  goals  {G^) 
(possibly  including  if),  we  consider  all  unions  of  the  subcollections  to  get  some 
measure  of  weakest  recognizability. 

12.4.  Strategy  Equivedence 

A  one-step  weak  EDR  strategy  is  not  very  interesting.  In  particular,  we  can 
always  obtain  one!  Surprisingly,  it  is  possible  to  define  a  way  of  coupling  two  weak, 
one-step  EDR  strategies  together  to  make  a  two  step  strategy  which  has  many  of 
the  characteristics  of  strong  EDR.  In  particular,  we  will  develop  a  way  of  making 
precise  the  idea  that  the  two  weak  EDR  steps  can  be  combined  to  make  a  two-step 
strategy  that  is  “equivalent”  to  a  one-step  strong  EDR  strategy. 

Suppose  the  commanded  motions  of  the  two  weak  EDR  steps  ^lre  6i  and  d^- 
The  essence  of  this  “equivalence”  lies  in  disambiguating  a  previous  motion’s  (^I’s) 
result  without  destroying  the  goal  state. 

Now,  let  R  be  the  steirt  region,  and  G  the  goal  as  usual.  Assume  without  loss 
of  generality  that  G  is  contained  within  the  forward  projection  of  R  under  6i  (see 
sec.  7.3  for  justification).  Let 


Ri  =  Rn  Ps^^F,^(R)iG). 
Now,  we  have  the  recognizability  diagrams 


(24) 


Ri 

R-Ri 


ei. 


G 

H 


> 


pusha  (G  U  H) 


G 

H' 


push(,,(G) 


G 


R 


fli 

/ 

\ 


push<,,(fr). 


(27) 


It  remains  to  ensiire  that  good  EDR  behavior  occurs  when  is  executed  from 
push9j(ff).  Now,  think  of  6i  *  62  bs  the  composite  strategy  formed  by  executing 
motion  9i  followed  by  62.  We  wish  to  find  additional  conditions  which,  together 
with  (25),  will  admit  both  the  fixed-point  diagram  (26)  eind  a  strong  EDR  diagram, 


R  (28) 

H'\ 

for  some  H"  (see  below).  Together  with  the  weak  EDR  diagram  (25)  (which  is 
tautologously  true  for  sticking  termination),  the  additional  conditions  below,  which 
we  will  call  the  linking  conditions,  are  necessary  and  sufiBcient  for  defining  an  equiv¬ 
alence  between  two  “linked”  weak  EDR  strategies  and  a  single-step  strong  EDR 
strategy,  whose  recognizability  diagram  is  given  by  (19),  (substituting  for  6). 
Henceforth,  let  0  ■=  B\. 


Definition:  If  the  fixed-point  diagram  (26)  holds  and  if  (25)  admits  a  strong  EDR 
diagram  (28)  in  which 

H''  =  {H'},  (29) 

then  the  motion  strategy  0\  *  62  is  said  to  be  strongly  equivalent  to  a  strong  EDR 
strategy  with  recognizability  diagram  (19). 

An  example  of  such  a  strategy  is  the  two-step  peg-in-hole  insertion  plan  with 
model  error,  figs.  4-66. 


Definition:  If  the  fixed-point  diagram  (26)  holds  and  if  (25)  admits  a  strong  EDR 
diagram  (28)  in  which 


H"  =  (30) 

then  the  motion  strategy  0^  *  02  is  said  to  be  weakly  equivalent  to  a  strong  EDR 
strategy  with  recognizability  diagram  (19). 


Note  that  we  define  (strong  or  weak)  equivalence  using  (19)  with  0  =  6i, 
not  with  6  =  $1  *  $2.  The  reason  for  this  is  as  follows.  If  0i  *  02  satisfies  the  weak 
equivalence  condition  (30)  and  the  fixed-point  diagram  (26),  then  after  termination, 
we  are  assured  that  the  outcome  of  0i  has  been  completely  diagnosed.  That  is, 
the  run-time  executor  knows  whether  or  not  terminated  in  success  or  failure. 
However,  it  is  not  necessarily  true  that  the  outcome  of  02  is  completely  diagnosed. 
This  occurs  in  the  worst  case,  if  if '  U  G  is  recognizably  attsiined.  We  discuss  this 
point  in  some  detail  below. 

The  following  gives  an  implicit  definition  of  Unking  condition-s: 

Definition:  Let  H"  be  chosen  for  either  strong  or  weak  equivalence,  as  in  (29) 
or  (SO).  The  Unking  conditions  are  necessary  and  sufficient  conditions  for  (25)  to 
admit  a  fixed-point  diagram  (26)  and  a  strong  EDR  diagram  ( 28 ). 

It  remains  to  show,  of  course,  that  Unking  conditions  exist  for  strong  or  weak 
equivalence.  We  wiU  momentarily  postpone  the  derivation  of  the  Unking  conditions 
in  order  to  describe  what  the  Unking  should  effect. 

Once  “Unked,”  two  one-step  weak  EDR  plans  should  admit  the  strong  EDR 
diagrsun  (28).  The  claim  is  that  (28)  is  in  some  sense  “equivalent”  to  the  strong 
EDR  diagram  (19).  How  is  this  possible? 

(19)  indicates  that  the  nm-time  executor  can  disambiguate  the  success  or  failure 
of  motion  0\.  The  same  is  true  of  strategy  0i  *  02  in  (28).  Here  the  possible 
results  of  executing  0i  *  02  when  the  steps  0i  and  02  are  properly  “Unked:” 

1.  G  is  achieved  imd  recognized  at  termination.  In  this  case,  either  (i)  achieved 
G  and  the  nm-time  executive  may  not  have  recognized  it,  but  02  disambiguated 
the  result  while  stiU  terminating  within  G.  Alternatively,  (ii)  0i  failed,  recurhing 
H,  and  02  subsequently  achieved  G  from  H. 

2.  H'  is  achieved  and  recognized  at  termination.  In  this  case,  0\  is  known  to  have 
failed,  and  the  robot  is  known  to  be  outside  G. 

(1)  and  (2)  are  the  only  outcomes  given  strong  equivalence.  With  weak  equivalence, 
a  third  outcome  in  also  possible: 

3.  G  U  H'  is  achieved  and  recognized  at  termination.  In  this  case,  0i  is  known  to 
have  failed. 

Thus  the  key  is  that  02  does  not  corrupt  the  goal  state;  that  is,  G  is  a  fixed 
point  under  02.  The  desirability  of  outcomes  (1)  and  (2)  are  clear.  One  might  ask, 
what  good  is  weak  equivalence?  Why  would  anyone  want  outcome  (3)?  The  answer 


is:  in  one-step  strong  EDR  (19),  the  run-time  executor  can  (a)  disambiguate  the 
result  of  motion  6i ,  and  (b)  in  case  of  failure,  know  that  the  robot  is  not  in  the 
goad.  In  weak  equivalence,  we  have  (a)  but  not  (b).  That  is,  in  outcome  (3),  we 
have  completely  diagnosed  the  result  of  motion  6i ,  although  in  the  process,  we  may 
have  aiccidentally  moved  into  the  goal.  That  is,  we  may  indicate  failure  when  we 
have,  in  fact,  succeeded.  However,  we  will  never  indicate  success  unless  it  is  certain. 
In  short,  when  linked,  9i  *  $2  is  “conservative”  about  declaring  success. 


12.5.  The  Linking  Conditions 


We  now  derive  the  linking  conditions.  Let 


=  Fs,  (R) 

push^^  =  pushjj(G  U  H) 

R2  =  push,,  n Pe^,F,^(G). 

The  overloading  notation  for  push,^  is  symmetric  with  that  for  preimages  and 
forward  projections:  both  the  map  and  its  image  are  denoted  by  the  scune  symbol. 
The  discussion  of  linking  conditions  assumes  sticking  termination.  However,  the 
derivation  goes  through  muiatia  mutandis  for  more  general  termination  conditions, 
if  we  let^ 


push,^  =  F.,,(R). 

It  remains,  however,  to  extend  the  linking-conditions  for  time-indexed  forward  pro¬ 
jections. 

We  now  demonstrate  our  claim  that  linking  conditions  exist. 


Definition:  The  condition  (LO)  is 


G  n  push,j  C  R2- 


Here  is  the  motivation  behind  (LO).  (LO)  says  that  whenever  motion  $i  ter¬ 
minates  in  the  goal  G,  then  the  state  is  inside  the  preimage  of  G  under  the  next 
motion  62.  The  intent  of  (LO)  is  to  admit  the  fixed-point  diagr^Lm  (26). 


Claim:  (LO)  implies  the  fixed- point  diagram  (26). 
Proof:  The  preimage  equation  for  (26)  is 


*  F*  is  defined  in  the  appendix.  See  also  sec.  12.2. 


■Pfl,,push,j(G)(<5)  =  push^j(G). 

This  preimage  is  taken  with  respect  to  a  smaller  start  region  than  il2-  D 

Note  however  that  the  converse  is  false.  (LO)  is  stronger  than  the  fixed- point 
diagram  (26),  since  the  preimage  R2  is  taken  with  respect  to  the  entire  forward 
projection  tmder  62. 

Claim:  Linking  conditions  exist,  and,  in  particular,  (LO)  is  a  linking  condition. 
Proof:  Suppose  (LO)  holds.  This  yields  the  following  reachability  and  recognizability 


diagram: 

linking  condition  (LO) 

Ri 

G  D 

^  A 

G  n  push^j  C  R2 

=  il2 

G 

/ 

R  —  Ri 

— ^  if  D 

if  n  pushgj  C  pushjj 

D  push^j  —  R2 

H' 

H'UG 

' - -  'V  ^ 

reachability  recognizability 

(31) 

To  see  that  diagram  (31)  demonstrates  weak  equivalence,  we  use  a  technique 
like  “diagram  chasing”  (see,  eg.,  [Hungerford]).  Assume  (LO)  holds.  Starting  from 
jRi ,  $1  effects  a  motion  reaching  G.  This  motion  in  fact  terminates  in  G  H  push^  . 
Since  by  (LO)  G  D  push^^  is  within  R2,  O2  then  effects  recognizable  termination  in 
G. 

On  the  other  hand,  if  the  motion  begins  in  R  —  Ri,  then  $i  effects  a  motion 
reaching  either  G  or  H.  If  G  is  reached,  then  O2  will  eventually  effect  recognizable 
termination  in  G,  by  the  argument  immediately  above.  If  H  has  been  reached,  then 
the  motion  0i  will  in  fact  terminate  at  some  point  z  in  H  H  push^  .  Then  there  are 
two  cases.  Case  (i):  z  €  R2-  Since  the  preimage  R2  is  constructed  with  respect 
to  the  entire  forward  projection  of  push^^ ,  motion  $2  will  next  effect  recognizable 
termination  in  G.  Case  (ii);  r  ^  i?2.  In  this  c£ise,  motion  02  will  effect  recognizable 
termination  in  one  of  {  G,  ff,  ff'  U  G  }. 

We  conclude  the  process  by  “forgetting”  all  the  intermediate  steps,  and  renam¬ 
ing  them  to  $1  *02.  First,  observe  that  the  fixed-point  diagram  (26)  holds.  Next,  to 
see  that  (31)  admits  an  EDR  diagram  (28)  in  which  (30)  holds,  we  remember  only 
the  start  region  R  and  the  “results”  G,  H',  and  H'  U  G.  Diagram  chasing  shows 
that  these  may  be  joined  with  recognizability  arrows  as  in  (28). 

Thus  the  diagram  (31)  demonstrates  weak  equivalence.  For  strong  equivalence, 
we  remove  if'  U  G  as  an  outcome  of  02-  Note  that  the  linking  condition  is  not  a 
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tautology.  However,  note  that  sdl  the  other  subset  relations  and  the  equality  in  (31) 
are  tautologous.  □ 

In  the  future,  we  will  leave  similair  diagram-cheising  arguments  to  the  reader. 
We  may  thus  conclude  that 

Theorem:  The  linking  condition  (LO)  is  a  necessary  and  sufficient  condition  for 
weak  equivalence  of  9i  *62  to  a  one-step  strong  EDR  strategy. 

Proof:  The  claims  above  have  demonstrated  siifficiency.  It  remains  to  show  (LO)  is 
necess2uy.  Suppose  (LO)  is  false,  but  (26)  still  holds.  (This  is  the  interesting  case, 
for  if  (26)  does  not  hold,  then  equivalence  cannot  possibly  follow).  (26)  says  that 
when  the  motion  is  known  to  start  within  push0^(G),  then  it  can  be  guaranteed  to 
terminate  recognizably  in  G.  The  antecedent  is  a  precondition  for  success  of  the 
motion.  After  di,  however,  this  precondition  may  be  false:  even  if  $i  reaches  G, 
it  is  only  known  to  have  reached  push^^.  In  particular,  (26)  says  nothing  about 
what  happens  when  62  is  executed  from  H.  (LO),  on  the  other  hand,  says  that 
termination  in  G  can  be  recognized  no  matter  where  62  originates  in  push^^ .  □ 

Now,  we  can  derive  some  equivalent  linking  conditions  that  are  somewhat  sim¬ 
pler  in  form.  Let 

i?;  «  i?2  n  G. 

Definition:  The  linking  conditions  (Ll)  and  (L2)  are 

G  n  pushj^  =  R2  (1^1) 

H  n  pushjj  =  ptish^j  —  R2  (L2) 

These  linking  conditions  admit  the  reachability  and  recognizability  diagram 

Unking  condition!  (Ll),  (L2) 


Ri  ^  G  D  Gnpushj,  =  G 

/-  / 

I  —  i2i  — ►  H  D  H  r\  pushjj  =  pushjj  —  R2  =>  H' 

s 

H'liC 


reachability 


Comments:  Let 


recognizability 


Pe,  =  'P9,,r,,(pu8h,j(G))(G), 


30  R2  =  push^^  n  P$,.  Note  that  (L2)  is  not  tautologous,  for  we  can  have  x  £  G, 
X  ^  P(»,  if  (LI)  is  false.  Therefore  x  €  push^^  —  and  x  ^  push^^  H  H. 

Lemma.  The  linking  conditions  (Ll)  and  (L2)  are  equivalent. 

Proof.  (Ll)  implies  (L2).  Suppose  (Ll).  Let  x  £  H  D  push^^.  x  £  R^  implies 
X  £  G.  Therefore  x  ^  H  is  &  contradiction.  Therefore  x  £  push^^  —  iZJ. 

Now  let  X  £  push^^  —  iJJ.  Therefore  x  ^  G  0  push^^  D  P#,.  Therefore  x  ^  G 
or  X  ^  Ptfj.  In  the  former  case,  x  £  H.  la  the  latter,  suppose  that  x  £  piish^^  and 
X  £  G  and  x  ^  P®,.  But  by  (Ll),  x  €  G  H  push^j  implies  i  6P.„  a  contradiction. 

(Ll)  if  (L2).  Let  x  £  G  0  push^^.  Show  x  £  R^.  We  need  only  show  that 
X  £  P$j.  Now,  X  ^  Pfl,  implies  x  £  H  r\  pushup  a  contradiction.  Now  let  x  € 
Therefore,  x  €  G  fl  push^^ .  □ 

Lemma:  The  linking  conditions  (LO)  and  (Ll)  are  equivalent. 

Proof  (LO)  implies  (Ll ).  Suppose  (LO),  i.e.,  G  fl  push^^  C  R2-  Show  G  H  push^^  = 
R^  =  G  n  Pj- 

Let  X  e  G  n  push^j.  Now,  (LO)  implies  that  x  €  P2.  Therefore  x  £  G  and 
X  £  R2.  Hence  x  €  P2. 

Let  X  €  PJ-  Therefore  x  €  G  H  Pj.  Hence  x  €  G  D  push^^  fl  P^,,  i.e.,  x  € 
G  n  push^j . 

(LO)  if  (Ll )  is  trivial.  0 

Theorem:  The  following  linking  conditions  are  equivalent: 


G  n  push^^ 

C  P2 

(LO) 

G  n  push^j 

=  P5 

(11) 

H  n  push^^ 

=  push^j  -  P5 

(12) 

*  12.6.  Beyond  the  Fixed-Point  Restriction 

In  the  discussion  above,  we  have  required  that  the  goEd  was  a  fixed  point  imder 
motion  $2.  We  now  discuss  how  to  relax  this  restriction.  In  particular,  it  is  possible 
to  extend  the  notions  of  strategy  equivalence,  and  the  linking  conditions,  to  the 
czise  where  a  subgoal  Gi  is  in  fact  the  preimage  of  the  actual,  or  final  goal.  Go, 
imder  62.  Thus  Gi  is  no  longer  the  fixed  point  of  ^2?  but  rather  the  preimage  of 
Go.  This  section  is  somewhat  technical  and  may  be  skipped  at  first  refiding.  We 
regard  relaxing  the  fixed-point  restriction  as  a  digression.  The  subsequent  material 
may  be  understood  even  if  this  section  is  omitted,  however,  the  reader  may  wibh  to 
bear  in  mind  that  such  a  generalization  does,  in  fact,  exist. 


We  consider  the  situation  where  from  R,  Oi  may  attain  Go  or  Gi,  where  “Gi  = 
Pe,(Go)”  However,  Gi  may  not  be  distinguishable  from  Go  imder  ^i.  Thus  the 
three  reachability  results  of  $i  are  Go,  Gi,  or  Hi,  where  Hi  is  the  EDR  region  for 
01  when  we  view  the  goal  as  Go  U  Gi. 

To  define  strategy  equivalence  in  the  non-fixed-point  case,  we  first  generalize 
the  fixed-point  diagram  (26)  as  follows. 

Definition:  The  generalized  fixed-point  diagram  is 


p\ash^j(Go  U  Gi)  Go. 


(33) 


Next,  we  modify  the  definitions  of  strategy  equivalence  and  the  linking  con¬ 
ditions  to  require  that  the  generalized  fixed-point  diagram  (33)  hold  in  place  of 
the  old  fixed-point  diagram  (26).  To  avoid  confusion,  we  will  call  (26)  the  simple 
fixed-point  condition. 

Now,  we  let 

ill  =  R  n  -Ptfi.F#,  (^'o  U  Gi) 
pushfl,  =  F»0^{R) 

Fe,  =  F(j,(pushjJ 
Poi  =  P9i,F»^{Go) 

R2  =  push®,  n  P®, 

Next,  define 

il^  =  Gynil2,  0  =  0,1) 

It  is  possible  to  generalize  the  definition  of  R^  and  the  linking  conditions  to  more 
than  two  subgoals  {  Gj  }.  We  would  do  this  by  writing  (Vj)  in  place  of  (7  =  0, 1). 

We  already  know  one  linking  condition: 


Pj,  D  Go  U  Gi.  {LZ) 

In  addition,  we  can  derive  the  following  linking  conditions.  Recall  Hi  is  the 
EDR  region  for  motion  0i,  viewing  the  goal  of  9i  as  Go  U  Gi. 


=  Pusho, 


push^^  n  Gj 

pushjj  n  Hi 


(V»  (II') 
(12') 


Comments:  Clearly  we  have  {L\')  implies  {L2').  However  I  have  not  been  able 
to  prove  the  converse  true.  I  suspect  it  is  false,  since  Go  may  intersect  Gj,  and  H21 
the  EDR  region  for  621  niay  intersect  Gi,  etc. 

Finadly,  note  that  all  three  linking  r  -iditions,  {Ll\L2',L3)  are  required  for 
the  composition  0i  *  62  to  admit  an  equivalent  strong  EDR  diagram.  This  points 
out  the  chief  theoretical  advantage  of  strategy  equivalence  with  the  simple  fixed- 
point  condition  (26).  With  the  simple  fixed-point  condition,  the  linking  conditions 
(LO),  (LI)  and  (L2)  were  found  to  be  equivalent.  With  the  generalized  fixed  point 
condition  (33),  not  only  do  the  corresponding  Unking  conditions  (LI')  and  (L2') 
appear  to  be  inequivalent,  but  we  also  require  the  additional  independent  condi¬ 
tion  (L3).  While  it  is  gratifying  that  our  key  concept — composing  two  weak  EDR 
strategies  via  Unking  conditions  to  admit  strategy  equivalence — in  fact  generaUzed 
to  the  non-fixed-point  case,  the  generalization,  unfortunately,  is  correspondingly 
more  compUcated. 

12.7.  What  Good  is  Weak  Equivalence? 

We  now  pose  the  following  question.  Why  is 

Ri  G 

R  —  Ri  ff*  (34) 

GUH' 

any  better  than 

Ri  G 

R-Ri  =»  H  (35) 

GU^ 

? 

• 

(35)  is  simply  the  weak  EDR  diagram  for  motion  9i.  It  always  holds  (given  the 
reachabiUty  diagram).  (34)  is  the  equivalent  recognizability  diagram  for  *$2  when 
a  linking  condition  is  satisfied.  That  is,  (34)  is  obtained  through  weak  equivalence. 
Why  is  (34)  stronger  than  (35),  and  would  one  prefer  (34)  to  (35)? 

Here  is  our  answer.  pushjj(G)  is  a  fixed-point  of  O2.  Therefore,  nothing  is 
“lost”  by  ^2-  ^2  serves  to  disambiguate  the  result  of  9i,  without  polluting  the  state. 
Second,  note  that  9i  *  ^2  is  “conservative”  about  declaring  success.  It  is  as  if  we 
u.sed  ^2  to  convert  the  reachability  diagram 


R-Ri 


H 


into  the  recognizabihty  diagram 

Rl  G  “Win” 

^  (37) 

R  —  Ri  =>  “Lose,  but  knowing  di  did  not  achieve  G” 

More  precisely,  the  “lose”  states  are 

H'  w  9i  did  not  achieve  G,  and  now  the  robot  is  outside  of  G. 

G  U  if'  at  6\  did  not  achieve  G,  and  now  we  might  be  in  if',  but  can’t  guarantee 
that  we’re  outside  of  G. 

On  the  other  hand  (35),  achieving  G  U  if  after  9i  only  tells  us  that  we  started 
in  R  —  Ri,  and  does  not  tell  its  the  result  of  motion  9i . 

12.8.  Application:  Failure  Mode  Analysis  in  the  Gear  Experiment 

We  now  discuss  how  the  failure  mode  analysis  used  to  generate  motion  92  in 
the  gear  domain  may  be  viewed  using  the  weak  EDR  theory. 

In  the  gear  meshing  plan,  9i  is  a  pure  translation,  and  92  is  a  pure  rotation. 
The  goal  is  a  fixed  point  under  92.  Consider  (32).  In  the  gear  plan,  the  reachability 
arc 


is  present,  but  the  arc 


R-Ri 


(38) 


^  (39) 

push^j  —  i?2 

is  not.  That  is,  it  is  possible  to  serendipitously  achieve  the  goal  under  translation 
but  not  rotation.  The  linking  conditions  are  satisfied.  Now,  is  the  outcome  G  U  ff ' 
possible?  Failure  mode  analysis  yields  the  answer:  No.  In  this  case,  9i  ♦  92  is 
strongly  equivalent  to  a  one-step  strong  EDR  strategy 


Rt 


Q 


R-Ri  =»  H'. 

The  full  reachability  and  recognizabihty  diagram  for  the  gear  plan  is  given  by 


linking  conditions  (LI),  (L2) 


G 


Ri  — ^  G  3  Go  push^j  —  R2 

/ 

R  —  Ri  — ^  H  D  H  r\  push^j  =  ptish^^  —  iZj  =>  H'  (40) 


' - » - 

reachsbility 


recognizsbility 


12.9.  Discussion  and  Review 


We  now  discuss  the  relationship  between  push-forward  algorithms,  failure-mode 
anzdysis,  and  the  weak  EDR  theory.  Recall  the  diagram  (32): 


linking  conditions  (Ll),  (L2) 

Ri  G  D  G  n  pushjj  = 

/ 

R  —  Ri  — ►  H  D  HD  push^j  =  push^^  —  RJ 


rescksbility 


a 

7 


recogniiability  0^ 


(41) 


(41)  is  the  full  reachability  and  recognizability  diagram  for  weak  equivalence. 
The  arrows  (a)-(d)  all  correspond  to  motion  62’,  we  have  labeled  them  so  as  to  be 
able  to  refer  to  them  in  the  discussion. 

Failure  Mode  Analysis.  The  reachability  £ind  recognizability  diagram  for 
failure  mode  analysis  (40)  is  found  by  deleting  arcs  (b)  and  (d)  from  (41).  In 
Limited,  arc  (a)  is  essentially  a  user  input^  (see  sec.  11.3).  The  failure  mode 
analysis  algorithms  Spec  and  Gen  decide  arc  (c).  Thus,  in  sec.  11.3,  (c)  corresponds 
to  (FI).  Failure  mode  analysis  links  a  weak  EDR  strategy  9i  followed  by  a  strong 
EDR  strategy  $2.  (a)  warrants  that  G  is  a  fixed-point  imder  62-  (b)  ensures  that 
failure  is  preserved  under  ^2-  serendipitous  goal  achievement  from  H  is  possible. 
Thus  such  plans  are  pure  disambiguation  strategies. 

Push-Forward  Algorithms.  Plans  found  by  push-forward  algorithms  such  as 
Multi  admit  a  diagram  from  (41)  containing  arcs  (a),  (b),  and  (c),  but  not  containing 

^Although  we  have  discussed  methods  for  inferring  (a)  computationally,  this  is  really  a 
direction  for  future  work  rather  than  a  focus  of  this  research. 


(d).  The  arc  (b)  (which  is  shown  in  detail  in  eq.  (39))  permits  serendipitous  goal 
achievement  from  H  under  62.  The  absence  of  arc  (d)  yields  strong  equivalence. 
Again,  push-forward  algorithms  link  a  weak  EDR  strategy  followed  by  a  strong  one. 
They  differ  from  f£ulure  mode  analysis  plans  in  that  the  arc  (b)  is  F>ermissible,  and 
(a)  is  not  a  user  input.  The  peg-in-hole  plan  with  model  error  (figs.  4-66)  is  an 
example  of  such  a  plan. 

2-Step  Weak  EDR.  A  plan  admitting  the  diagram  (41)  with  all  four  arcs 
(a)-(d)  demonstrates  weak  equivalence.  It  is  formed  by  linking  together  two  weak 
EDR  strategies  into  a  2-Step  plan.  We  have  discussed  the  semantics  of  such  plans 
above.  The  key  differences  between  2-step  weak  EDR  plans  and  push-forward  or 
failure-mode  plans  are  (1)  the  existence  of  arc  (d),  and  (2)  the  linking  of  2  weak  (as 
opposed  to  a  weak  and  a  strong)  EDR  strategies. 

In  all  cases,  note  that  the  linking  conditions  are  required.  Thus  the  linking  con¬ 
ditions  have  somewhat  surprisingly  turned  out  to  be  the  tmderlying  characterization 
for  multi-step  EDR  strategies.  That  is,  since  they  are  necessary  and  sufiicient  con¬ 
ditions  for  constructing  multi-step  EDR  plans,  the  linking  conditions  may,  in  fact, 
be  taken  as  the  definition  of  multi-step  EDR  strategies. 

Hence  in  considering  LlMlTED’s  techniques  for  multi-step  strategy  generation, 
we  find  that  both  failure  model  analysis  and  push-forwEurd  algorithms  are  essentially 
special  cases  of  the  Weak  EDR  theory.  This  is  summarized  in  the  table  below: 


Method 

Arcs  in  (41) 

Strategy  Type 

Comments 

Failure  Mode  Analysis 

a,c 

weak*strong 

Pure  Disambiguation. 

(a)  is  ijser  input,  (c)  is  computed. 

Push-Forwards 

a,b,c 

weak*strong 

(b)  permits  serendipitous  goal  achievement 

Weak  EDR 

weak*weak 

2-Step  Weak  EDR. 

12.9.1  Algebraic  Considerations 

Let  us  pause  and  review  the  key  points  in  this  development.  Weak  EDR  the¬ 
ory,  strategy  equivalence,  and  the  linking  conditions  were  introduced  as  a  tmifying 
framework  for  planning  multi-step  strategies. 


1.  The  linking  conditions  are  necessary  and  sufficient  criteria  for  admitting  the 
composition  of  two  weak  EDR  strategies  di  *  and  ^2*  into  a  two-step  strategy  which 


3 

i 


I: 

'll 

i 

I 


is  weakly  equivalent  to  a  one-step  strong  EDR  strategy  from  9i.  We  may  write  this 
as 


9^*02'^  ~  ^1*  (42) 

2.  The  linking  conditions  are  necessary  and  sufficient  criteria  for  Euimitting  the 
composition  of  a  weadc  EDR  strategy  and  a  strong  EDR  strategy  ^2*  into  a 
two-step  strategy  which  is  strongly  equivalent  to  a  one-step  strong  EDR  strategy 
from  .  We  may  write  this  as 


9i'^*92^  ~  ^1*  (43) 

3.  The  gear  plan  is  a  special  case  of  (2).  In  particular; 

4.  Failure  mode  analysis  is  a  special  case  of  satisfying  the  linking  conditions  to 
render  a  two-step  EDR  strategy  strongly  equivalent  to  a  one-step  strong  EDR  strat¬ 
egy. 

5.  Multi-step  strategies  may  also  be  planned,  by  repeatedly  pushing  forward.  This 
was  the  gist  of  algorithm  Multi  in  the  beginning  of  this  chapter.  Multi  may  be  viewed 
as  chaining  together  weak  EDR  strategies  followed  by  a  strong  EDR  strategy.  Multi 
is  also  essentially  a  special  case  of  (2),  with  the  goal  fixed-point  condition  relaxed,^ 

We  can  view  the  set  all  strategies  4^  as  a  monoid  imder  the  composition  oper¬ 
ation  *.  The  generators  of  the  monoid  are  {9^  )  U  {  9\  }.  Strategy  equivalence  is  a 
way  of  defining  certain  relations  between  products  of  these  generators.  When  the 
linking  conditions  are  satisfied,  then  these  relations  take  the  form  of  (42)  for  weak 
equivalence  or  (43)  for  strong  equivalence. 

However,  we  carmot  directly  define  a  new  monoid  by  taking  the  quotient  of  $ 
by  these  relations.  This  is  because  the  relations  axe  not  always  true,  that  is,  they 
only  hold  when  the  linking  conditions  are  satisfied.  We  can  remedy  this  by  viewing 
#  as  “a  groupoid  without  inverses.”  We  call  such  a  structme  a  monoidoid.  That  is, 
the  operation  *  turns  out  to  satisfy  properties  that  look  very  much  like  the  axioms 
for  a  monoid.  These  are  called  the  monoidoid  properties  of  *.  The  only  difference 
from  the  properties  of  a  monoid  is  that  *^2  is  not  defined  for  every  pair  of  classes, 
but  only  for  those  pairs  9i,  02  for  which  the  linking  conditions  hold. 


4 


it 


,*• 

tv 


^Relaxing  this  restriction  was  discussed  in  the  section  “Beyond  the  Fixed  Point  Restric¬ 
tion,”  above. 
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IV.  Planning  Sensing  and  Motion  for  a  Mobile  Robot 


We  now  consider  an  application  of  the  EDR  theory  to  planning  sensing  and 
motion  for  a  mobile  robot  amidst  partially-known  obstacles.  A  partial  “map”  of 
the  environment  is  represented  using  generalized  configuration  space.  We  assiune 
that  the  robot  has  a  depth  sensor  which  it  can  use  to  interrogate  the  environment. 
We  call  this  process  active  sensing. 

Applying  EDR  to  the  mobile  robot  domain  yields  certain  insights  into  the  struc- 
tme  of  the  EDR  theory.  Conversely,  this  chapter  obtmns  a  technique  for  planning 
motions  and  active  sensing  for  a  mobile  robot  in  a  partially  known  environment. 
This  technique  provides  a  principled  approach  to  motion  planning  with  active  sens¬ 
ing.  It  shows  how  to  incorporate  a  more  fine-grained  model  of  sensing  into  the  EDR 
planning  framework. 

Much  work  remains  to  be  done.  In  particular,  the  EDR  freimework  for  ac¬ 
tive  sensing  is  still  fairly  theoretical.  Mobile  robot  environments  are  often  highly 
unstructured  [Brooks,  85],  and  representing  this  geometric  imcertainty  using  gener¬ 
alized  configuration  space  presents  a  non-trivial  problem.  Furthermore,  it  may  be 
impractical  to  model  more  general  vision  or  sonar  sensors  without  further  enhance¬ 
ments  to  the  EDR  theory.  More  study  is  required;  hopefully  this  theory  of  EDR 
planning  with  active  sensing  can  provide  a  starting  point. 
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13.  Planning  Sensing  and  Motion  for  a  Mobile  Robot 


Model  error  is  a  key  theme  in  this  thesis.  One  important  domain  in  which  there 
is  uncertainty  in  the  geometry  of  the  environment  arises  in  planning  motions  for  a 
mobile  robot.  Typically,  such  a  robot  must  plan  motions  amidst  partially  known 
obstacles.  Since  this  partial  knowledge  can  be  represented  ais  model  error  in  our 
generalized  configuration  space  framework,  it  is  natural  to  consider  EDR  planning 
in  this  domain. 

The  use  of  sensing  in  [LMT]  plans  might  be  characterized  as  “passive.”  In  each 
step  of  the  plan,  a  nominzd  applied  force  is  commanded,  and  the  position  and  force 
sensors  are  monitored  imtil  some  termination  conditions  are  satisfied.  At  this  time 
a  new  motion  is  selected,  and  so  forth.  This  model  of  sensing  and  action  arises  quite 
naturally  in  developing  compliant  motion  plans  for  assemblies. 

A  mobile  robot  plan,  however,  is  typically  not  limited  to  motion  commands. 
The  vision  or  sonar  sensors  on  a  mobile  robot  may  be  pointed  in  a  direction  and 
information  gathered  about  the  environment.  This  is  a  more  fine-grained  model 
of  sensing  than  is  currently  available  within  the  EDR  planning  framework.  In  this 
chapter,  our  goal  is  to  extend  the  EDR  framework  to  planning  both  motion  and 
sensing  actions.  We  develop  the  extension  in  the  mobile  robot  domain,  although 
it  is  applicable  to  any  domain  where  the  robot  has  a  choice  of  sensing  modalities 
and  directions  to  interrogate.  The  basic  difference  is  that  in  the  [LMT]  framework, 
motion  and  sensing  are  inextricable.  In  the  active  sensing  framework  described 
here,  we  assume  it  is  possible  to  sense  without  moving.  In  particular,  we  assume 
the  following  model  of  sensing  and  motion: 

Motions  of  the  mobile  robot  are  modeled  in  the  standard  way,  using  generalized 
configuration  space  with  sensing  and  control  error: 

•  The  mobile  robot  is  represented  by  a  polygon  moving  with  three  degrees  of 
freedom  x,  y,  d  in  the  plane.  There  are  partially  known  obstacles  in  the  plane, 
and  they  are  represented  using  the  space  of  geometric  variations  J.  ^  is  of 
course  the  product  of  the  Euclidean  group  acting  on  the  plane,  and  J.  Pushing 
of  the  obstacles  across  J  can  also  be  represented.  The  mobile  robot  has  a 
priori  position  sensing  of  accuracy  Cep,  control  accuracy  of  Cjo  ®^nd  force-sensing 
accuracy  of  eg„.  We  assume  that  the  robot  can  slide  on  surfaces  as  subject  to 
the  coefficient  of  friction  fi. 

Next,  we  eissume  the  following  additional  sensing  capability: 

•  The  robot  has  a  sensor  similar  to  a  laser  range-finder.  It  can  be  pointed  in 
any  direction  <i>  to  ascertain,  approximately,  the  distance  to  an  obstacle  in 
that  direction.  The  sensor  also  gives  the  approximate  surface  normal  of  the 
obstacle  patch.  The  aim  of  the  sensor  is  inaccurate;  however,  it  is  bounded 
by  a  cone.  The  aiming  inaccuracy  cone  is  defined  by  €a»m-  The  error  in  the 
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distance  measurement  by  the  sensor  is  bounded  by  edist-  The  error  in  normal 
measurement  is  bounded  by  e„. 

The  sensor  czm  be  pointed  and  aimed  to  ascertain  the  distance  and  orientation 
of  a  partially-known  obstacle  surface.  The  idea  is  that  by  choosing  where  and  when 
to  point  the  sensor,  the  robot  can  gather  information  about  the  geometry  of  the 
environment.  This  information,  in  turn,  can  be  used  to  infer  both  the  position 
of  robot  amd  boimds  on  the  possible  geometries  of  the  environment.  That  is,  the 
action  of  pointing  the  sensor  and  taking  a  “view”  provides  constraint  on  the  current 
position  both  in  C  and  in  J. 

Of  course,  it  is  possible  to  model  such  a  sensor  naively  within  the  [LMT]  preim¬ 
age  framework.  For  example,  one  could  assume  that  the  sensor  continuously  takes 
views  of  the  environment,  in  all  directions  at  once.  Then  one  could,  in  principle, 
obtain  upper  bounds  on  the  position  sensing  accuracy  that  can  be  inferred  from 
these  views.  These  upper  bounds  could  then  be  incorporated  into  the  position 
sensing  accuracy  of  the  robot. 

We  refine  this  model  as  follows.  We  assume  that  it  is  not  feasible  to  take 
continuous  views  in  all  directions  at  once.  In  particular,  we  assume  that  only  a 
discrete  niimber  of  views  may  be  taken,  and  that  the  robot  must  choose  where 
(in  the  plan)  to  take  them,  and  in  what  (relative)  direction.  Thus  the  primitive 
operations  available  to  the  robot  are  of  two  types: 

•  Motion  Commands,  of  the  form  “Move  in  heading  {x,y,$)  until  (termination 
condition).”  These  are  the  standard  kinds  of  motion  commands  discussed  in 
the  development  of  the  [LMT]  and  EDR  framework.  The  termination  condition 
is  the  usual  termination  predicate. 

•  Sensing  Commands,  of  the  form  “Point  the  sensor  in  direction  0  eind  take  a 
view.”  This  returns  an  approximate  distance  and  surface  normal  reading. 
Sensing  commands  are  always  executed  at  rest.  We  define  a  motion  plan  with 

active  sensing  to  be  a  sequence  of  motion  commands  interspersed  with  sensing 
commands.  As  usual,  conditional  branches  are  possible.  An  EDR  plan  with  active 
sensing  is  a  motion  plan  with  active  sensing  that  is  an  EDR  plan.  The  question 
is:  how  can  active  sensing  be  incorporated  into  the  EDR  framework?  We  will 
answer  this  question  by  showing  how  to  generate  EDR  plans  with  active  sensing. 
As  a  corollary,  we  obtain  a  technique  for  generating  guaranteed  plans  with  active 
sensing. 

The  key  idea  is  to  define  a  kind  of  “equivalence”  between  sensing  and  motion. 
Informally  speaking,  active  sensing  is  like  moving  up  to  an  obstacle,  measuring  the 
distance  traversed  and  the  normal  there,  and  then  moving  back  to  one’s  original 
position.  At  that  point,  by  consulting  an  approximate  “map”  of  the  environment 
and  using  dead  reckoning,  a  better  estimate  both  of  one’s  position  and  a  revised 
“map”  can  be  obtained.  Using  this  “equivalence”  between  sensing  and  motion,  we 


then  can  represent  both  motion  and  active  sensing  in  a  single  generalized  configu¬ 
ration  space.  In  this  space,  both  motion  and  active  sensing  sire  represented  as  kinds 
of  “generalized  motions.”  This  representation  permits  the  planner  to  treat  sensing 
and  motion  vmiformly.  More  precisely; 

•  Wt  describe  a  reduction  of  ike  problem  of  EDR  planning  with  active  sensing 
to  (ordinary)  EDR  planning  in  a  larger  generalized  configuration  space,  that 
represents  both  motion  commands  and  sensing  commands  as  “generalized  mo¬ 
tions.  ” 

This  reduction  is  computational;  it  is  similar  in  fiavor  to  the  reduction  of  plan¬ 
ning  with  model  error  to  computing  preimages  in  a  higher-dimensional  generalized 
configuration  space. 

It  is  now  our  task  to  make  precise  this  notion  of  the  “equivalence”  between 
sensing  and  motion.  To  develop  this  notion  without  clutter,  we  will  initisdly  simplify 
the  problem  as  follows.  Assume  that  the  robot  is  a  point  robot,  and  that  rotations 
need  not  be  considered.  Furthermore,  we  prohibit  pushing  across  J,  the  sp>ace  of 
model  uncertainty.  Hence  the  robot’s  configuration  space  C  is  simply  the  plane. 
The  reduction  of  the  robot  to  a  point  is  of  course  jtistified  by  existing  configuration 
space  formalisms.  We  will  reintroduce  rotations  later  after  describing  the  basic  idea. 

First,  we  introduce  the  definition  of  a  generahzed  configuration  space  planning 
universe.  A  planning  universe  is  a  tuple 

{S  »  ^  5  Ip) 

consisting  of 

a  generalized  configuration  space  Q  =  C  'x.  J, 

a  set  of  generalized  configuration  space  obstacles  O  =  {  Off  }, 

the  position  sensing,  velocity  sensing,  and  control  uncertainties,  ep,e„,ec- 

the  coefficient  of  friction  p, 

and  the  termination  predicate  tp. 

The  planner  can  plan  motions  in  this  universe  iising  the  EDR  framework  described 
earlier,  tp  represents  the  termination  predicate  available  to  the  run-time  executive. 
For  example,  we  might  have  the  [LMT]  termination  predicate,  which  uses  position- 
and  force-  sensing,  as  well  as  time  and  history.  Other  termination  predicates  include 
that  of  [Mason],  which  remembers  a  continuous  history  of  sensed  positions  and 
velocities,  amd  stick,  which  terminates  based  on  sticking. 

Now,  we  assume  that  our  initial  planning  problem  is  given  by  the  tuple 

Um  =  {Q,0,  ^ec)  M)  ip)  (1) 

where  Q  is  simply  3?^  x  J  as  described  above  to  define  the  space  of  motions  for  the 
mobile  robot.  This  may  be  thought  of  as  the  “motion  universe.”  We  wish  to  extend 
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Fig.  1.  Schematic  illustration  of  the  framework  for  EDR  planning  with  active 


this  universe  to  incorporate  active  sensing.  We  can  do  this  by  '^ad joining”  a  “sensing 
universe.”  Motions  planned  in  the  motion  universe  Um  correspond  to  physical 
motions  of  the  robot,  like  “Move  in  heading  (x,  y).”  “Generalized  motions”  in  the 
sensing  viniverse  corrapond  to  active  sensing  commands,  like  “Look  in  direction  ^ 
and  take  a  view.”  In  addition,  the  planner  needs  some  special  actions  that  move 
between  the  sensing  and  motion  universes.  While  these  actions  have  no  real  physical 
significance,  they  may  be  thought  of  as  “preparing  to  move”  or  “preparing  to  sense.” 
We  must  now  define  the  “sensing  universe.” 

The  sensing  universe,  which  we  call  Us,  contains  the  same  obstacles  as  the 
motion  universe  (1).  The  sensor  can  be  aimed  in  this  imiverse.  Once  the  sensor 
is  aimed,  we  imagine  that  a  “line  of  sight”  motion  Movt{<f>)  is  commanded.  This 
motion  terminates  on  the  first  surface  it  hits.^  Here  is  the  idea:  this  line  of  sight 
motion  can  be  modeled  as  a  generalized  damper  motion  with  control  error  Caimt  the 

^  Assuming  no  mirrors! 


aiming  accuracy  of  the  sensor.  That  is,  if  the  nominal  sensor  aiming  direction  is  <j>, 
then  the  effective  line  of  sight  motion  is  actually  in  some  direction  in  the  angular 
interval  (<^  —  Caimt^  +  ^aim)-  Once  the  motion  terminates,  the  distance  traversed 
can  be  measured  with  accuracy  tdiat-  The  normal  on  the  surface  at  the  point  of 
“contact”  can  be  measiured  with  acciuncy  Cn.  The  imcertainty  bounds  tdiat  and 
will  be  taken  to  be  the  position-  and  force-  sensing  imcertainties,  resp.,  in  the 
sensing  imiverse. 

Thus  we  construct  a  sensing  universe  whose  imcertainties  are  given  by  the  error 
characteristics  of  the  sensor.  We  next  provide  the  planner  with  the  primitive  com¬ 
mand  Move(S),  which  moves  from  the  motion  imiverse  into  the  sensing  imiverse, 
retaining  the  same  position  (in  Q)  relative  to  the  obstacles  O.  Once  in  the  sensing 
universe,  it  is  then  possible  for  the  planner  to  command  the  generalized  motion 
Move{<i>). 

We  have  said  that  active  sensing  was  like  “moving  to  an  obstacle,  metisuring 
its  distance  and  normal,  and  then  moving  back  to  the  original  position.”  So  far 
we  have  sketched  how  the  sensing  universe  models  the  mot?  m  up  to  the  obstacle. 
Now  we  must  describe  a  “return  universe”  which  models  the  motion  back  to  the 
original  position.  The  return  universe  is  perfect:  there  is  no  control  error.  However, 
the  only  termination  predicate  available  is  pure  position-sensing  associated  with 
the  command  Moveto(xo,yo),  where  (a:o»yo)  indicates  the  actual  position  of  the 
robot  before  the  Move{S)  command.  That  is,  in  the  return  universe,  there  is 
perfect  position  control,  but  the  only  position  that  can  be  commanded  is  the  original 
position. 

Thus  we  can  define  the  following  commands  which  are  available  to  the  planner 
to  use  in  its  motion  strategies. 

List  of  Generalized  Motions  Commands 

1.  Physical  Motion  Commands.  Same  as  “Motion  Commands”  above.  Ap¬ 
plicable  at  any  time  in  universe  Um. 

2.  Move(S).  Applicable  at  any  time  in  universe  Um-  First,  record  the  actual 
position  of  the  robot  in  the  variable  (xo,yo).  Next,  move  from  the  motion 
universe  into  the  sensing  universe,  retaining  the  same  position  (io>yo)  relative 
to  the  obstacles  O. 

3.  Move(0).  This  commeinds  a  straight  line  motion  in  relative  direction  <f>,  subject 
to  aiming  inaccuracy  (aim-  When  the  motion  terminates  on  the  first  surface 
struck,  the  sensing  uncertainty  bounds  tdiat  and  Cn  provide  a  characterization 
of  how  accurately  the  distance  to  the  obstacle  and  its  normal  may  be  measured. 
Note  that  the  line  of  sight  motion  effected  by  Move{(f>)  does  not  move  across 
J.  It  moves  in  C  and  retains  the  same  position  in  J.  Applicable  only  after  a 
Move{S)  command. 


4.  Move(R).  Move  from  the  sensing  tmiverse  into  the  return  universe,  retaining 
the  same  position  relative  to  the  obstacles  O.  Applicable  only  after  a  Move(<f>) 
command. 

5.  Moveto(xoi  l/o)<  Move  with  perfect  accmracy  from  wherever  the  robot  is  to 
(a:o5  yo)i  where  (xq,  j/o)  is  the  value  stored  by  the  last  Move(S)  command.  That 
is,  (xo,yo)  is  a  literal  here,  and  may  not  be  chosen  by  the  planner.  Applicable 
only  after  a  Move{<t>)  conunand. 

6.  Move(M).  Move  from  the  return  universe  into  the  motion  \iniverse,  retaining 
the  same  position  (xo,yo)  relative  to  the  obstacles  O.  Applicable  only  after  a 
Mot;e<o(xo,  yo)  command. 

We  now  make  the  construction  somewhat  more  formal. 

The  sensing  universe  Us  defined  to  have  the  same  obstacles  as  Um-  How¬ 
ever,  the  uncertainties  in  Us  correspond  to  the  error  bounds  in  aiming  the  sensor, 
and  in  measuring  the  distance  and  normal  to  an  obstacle.  To  construct  Us,  first 
assume  that  Cdist  =  0  and  €„  =  0  that  is,  assume  a  perfect  sensor  which  is  aimed 
inaccurately.  Then  we  could  construct  Us  as 


y. 


Us, perfect  ~~  {Q  ,^,0,0,  €aim,  • 

In  the  sensing  universe,  the  coefficient  of  friction  is  infinite.  Hence  the  line  of  sight 
motions  which  terminate  on  the  first  obstacle  they  strike  are  exactly  modeled  by 
damper  motions  which  stick  on  any  surface.  The  termination  predicate  halts  such 
motions  as  soon  as  they  stick,  that  it,  as  soon  as  they  make  contact. 

The  point  is  that  with  a  per/ec<  sensor,  the  motion  Move{<j>)  terminates  exactly 
on  the  surface  W  it  strikes.  However,  for  an  imperfect  sensor,  this  motion  must  be 
modeled  as  terminating  within  of  this  surface.  That  is,  the  motion  terminates 
within  the  set  W  0  where  Bdut  is  a  ball  of  radius  edist-  We  can  model  this 

termination  via  a  “jerky”  termination  predicate  which  stops  on  the  first  surface  it 
hits  (using  sticking),  md  then  “jerks”  away  some  distance  no  greater  than  edist 
before  halting.  We  denote  this  termination  predicate  as  stick  ±  Cdijj.  Hence,  in 
general. 

Us  =  {Q,  0,€diet,  €n>  ^aim,  (X),stick±€di,t)-  (2) 

Recall  that  push^(-)  denotes  the  a  priori  push-forward  based  on  sticking.  Sup¬ 
pose  that  the  initial  position  of  the  robot  is  known  to  lie  in  some  start  region  R. 
Then  with  a  perfect  sensor,  Move((f>)  simply  terminates  within  push^(il).  push^(i2) 
is  identical  to  the  obstacle  edges  of  the  forward  projection  of  R  under  (j)  subject  to 
control  uncertainty  eaim  and  /x  =  oo.  With  an  imperfect  sensor,  Move(<f>)  termi¬ 
nates  within  the  set 
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The  retvirn  iiniverse  Ur  also  has  the  sajjie  obstzwjles.  In  it  we  have  perfect 
control  and  sensing: 

Ur  =  {Gi  0, 0,  — ,  0,  — ,  pure  position  control).  (3) 

Of  course,  in  Ur  we  are  only  permitted  to  command  one  motion;  the  motion  re¬ 
turning  to  (xQ,yQ).  In  both  the  sensing  and  the  return  universes,  Q  is  again  3?^  x  J. 

EDR  planning  with  active  sensing  may  be  regarded  as  a  planning  problem  in 
the  larger  generalized  configuration  space 

Um^UsU  Ur.  (4) 

We  regard  this  generalized  configiiration  space  as  endowed  with  a  special  “physics” 
that  governs  motions  in  the  three  imiverses  it  comprises,  and  how  the  robot  can 
in  faet  move  between  universes.  In  addition,  of  course,  the  planner  must  satisfy 
certain  compositional  constraints  in  constructing  pl2ins.  That  is,  certain  steps  are 
only  applicable,  or  valid,  when  preceded  by  other  steps.  This  is  a  constraint  on  the 
type  of  operators  available  to  the  planner  when  it  chooses  commanded  generalized 
motions.  For  example,  in  the  physical  motion  universe  U m  » there  is  a  choice  between 
a  physical  motion  command  {in  Um)  and  a  Move{S)  command  to  enter  the  sensing 
universe.  But  once  the  sensing  universe  has  been  entered,  there  are  no  choices  in 
the  type  of  operator  to  apply,  but  merely  in  their  peu’ameterization  (specifically,  the 
choice  of  <^). 

For  this  reason,  any  implementation  of  EDR  planning  with  active  sensing 
should  combine  operators  (2-6)  into  a  single  operator 

(2-6).  Sensor(^).  Command  the  sequence  of  generalized  motions:  Move{S), 

Move{(f>),  Move{R),  Moveto{xQ,yQ),  Move{M). 

The  operator  Sensor{<f>)  is  the  formal  model  for  Sensing  Commeinds  (as  defined 
above  in  boldface).  The  reason  we  decomposed  this  operator  into  steps  (2-6)  was 
to  illustrate  the  structure  of  the  problem,  and  to  show  how  active  sensing  could  be 
integrated  into  motion  planning  with  imcertainty,  using  familiar  tools  in  the  EDR 
framework. 

This  completes  the  reduction  for  the  special  case  of  point  robots  in  the  plane. 
We  will  now  provide  an  example,  and  then  return  to  generalize  the  reduction  to 
non-point  robots  with  rotational  degrees  of  freedom. 


Fig.  2.  Detail  of  the  generalized  motion  Move{^)  in  U$.  The  start  region  is  R. 
The  forward  projection  is  the  outer  envelope  of  all  pouible  lines  of  sight,  given 
the  aiming  error  of  the  sensor.  The  push>forward  is  the  wall  of  Oz  the  sensor 


can  see. 


13.1.  Using  Information  Provided  by  Active  Sensing 

In  this  section  we  clarify  how  the  additional  information  provided  by  active 
sensing  is  used  by  the  planner  to  further  constrain  the  position  of  the  robot  in  gen¬ 
eralized  configuration  space.  While  the  incorporation  of  this  constraint  is  implicit 
in  the  reduction  above,  it  helps  to  see  an  explicit  construction  in  an  example.  This 
example  builds  on  fig.  1.  The  development  here  is  somewhat  informs) 

Suppose  that  the  robot  is  known  to  lie  in  some  region  R  in  generalized  con¬ 
figuration  space.  For  example,  in  fig.  2,  R  is  the  same  size  as  the  position  sensing 
uncertainty  ball  Btp.  We  wish  to  calculate  explicitly  how  taking  a  view  in  direction 
<!>  can  further  constrain  the  possible  positions  of  the  robot.  For  example,  by  point¬ 
ing  the  sensor  at  a  wall  of  obstacle  O3  in  fig.  2,  the  robot  may  be  able  to  further 
localize  its  position,  given  some  information  about  the  distance  and  oricutation  of 


'M 


Fig.  3.  AfUr  th«  generaJiMd  motion  Af  ove(^)  terminate*,  the  robot  is  known 
to  lie  within  distance  of  the  sticking  push>forward.  This  defines  a  tube  in 
generalized  configuration  space.  The  information  effect  of  the  perfect  “return” 
motion  Moveto{x(i,yo)  ^  translate  this  tube  back  to  the  original  position. 
The  run-time  executive  now  has  better  localized  the  position  of  the  robot. 


the  wall. 

A  line  of  sight  in  direction  0  is  considered  to  be  like  a  straight  line  motion.  We 
must  consider  all  lines  of  sight  that  are  possible,  given  the  aiming  inaccuracy  of  the 
sensor.  We  view  all  possible  lines  of  sight  as  a  region  in  generalized  configuration 
space.  This  region  is  the  forward  projection  F^{R).  No  sliding  is  possible  in  the 
forward  projection,  since  the  coefficient  of  friction  is  infinite.  The  push-forward 
(based  on  sticking)  of  the  generalized  motion  Move{<f>)  is  part  of  the  wall  of  obstacle 
O3.  This  region  is  denoted  push^(iZ)  in  fig.  2. 

An  upi>er  bound  on  the  nm-time  executor’s  knowledge  of  the  wall’s  position  is 
fotmd  as  follows.  Let  Butt  denote  the  size  of  the  position  sensing  imcertainty  ball 
in  Us’  This  ball  is  an  upper  bound  on  the  sensor’s  ability  to  localize  distances.  The 
knowledge  of  the  wall’s  position  will  lie  within  the  convolution  of  the  push-forward 


of  the  generalized  motion  Move{<f>)  by  Bdut,  that  is, 


push^(il)  ©  Bdisf  (5) 

Hence,  after  executing  motion  (f>,  the  virtual  robot  is  guaramteed  to  lie  within  dis¬ 
tance  Cdist  oi  the  wall  O3. 

Now  suppose  the  actual  position  of  the  physical  robot  is  (xo>yo)»  as  in  fig.  2. 
Then  the  information  effect  of  the  command  Moveto(xo,yo)  is  to  translate  the  set 
(5)  back  to  the  original  position.  We  denote  this  operation  by^  trans(j;p_y,)(-).  Thus 
eifter  the  sensing  operation  Sensor{<f)),  the  position  of  the  robot  is  known  to  lie 
within  the  set 

R  n  tranS(,„,„,)(push^(J2)  ©  Bjut).  (6) 


♦  13.1.1  Using  Normtd-Sensing  Information 

The  detailed  starred  sections  below  may  be  skipped  at  first  reading. 

The  sensor’s  ability  to  detect  surface  normed  orientation  provides  additional 
constraint.  That  is,  by  sensing  the  normal,  the  sensor  can  further  localize  the  point 
of  cont2u:t  within  eq.  (5).  Recall  that  ir  denotes  the  canonical  projection  of  phase 
space  onto  position  space.  Then  the  localization  provided  with  combined  distance¬ 
sensing  and  normal-sensing  is  found  as  follows.  Let  denote  the  normal-sensing 
error  ball  of  radius  Cn  in  phase  space.  Now,  let  F^{-)  denote  the  forward  projection 
in  phase  space.  From  the  phase-space  forward  projection,  we  derive  the  a  priori 
phase-space  push-forward  based  on  sticking,  push^(-).  We  can  view  an  element  of 

pnsh0(i2)  as  a  pair,  consisting  of  a  point  and  a  tangent  vector.  Suppose  that  n* 
denotes  the  normal  as  sensed  by  the  sensor  at  execution  time,  and  let  n*"*"  be  its 
orthogonal  complement.  Thus  J3„  ©  n*-*-  represents  the  set  of  all  possible  actual 
tangent  vectors  consistent  with  n*.  Then  after  termination,  the  motion  Move{<f>) 
is  known  to  be  within  the  set^ 

Bdist  ©  7r^push^(R)  n  X  (B„  ©  n*'^)))^.  (7) 

Eq.  (7)  replaces  (5)  when  normal  sensing  is  available  to  the  sensor. 

♦  13.2.  Generalizations 

We  now  relax  some  of  the  initial  simplifying  assumptions  adopted  above.  In 
particular,  we  generalize  the  framework  for  EDR  planning  with  active  using  to 

*The  definition  of  trans  is  informal. 

^Formally,  the  notation  in  eq.  (7)  assumes  that  Q  is  parallelizable. 


the  case  of  a  polygonal  robot  moving  with  three  degrees  of  freedom  in  the  plane, 
amidst  partially-known  obstacles.  Some  of  the  obstacles  may  be  pushed  by  the 
robot,  which  may  ch^lnge  their  position  and  orientation. 

*  13.2.2  Pushing 

First,  we  incorporate  pushing  in  the  model,  as  follows.  Physical  motion  com¬ 
mands  (in  the  physical  universe  Um)  are  permitted  to  cause  pushing  of  movable 
obstacles,  resulting  in  cross-coupled  motions  in  C  and  J .  However,  the  operator 
Move{4>)  can  of  cotirse  cause  no  motion  in  J,  since  it  corresponds  to  vision  sensing. 
Hence  we  must  simply  restrict  the  effect  of  the  straight-hne  motion  Moue(0)  to 
motion  exclusively  in  C. 

*  13.2.3  Non-Point  Robots 

Next,  we  wish  to  consider  robots  which  are  not  points,  for  example,  polygonal 
robots  in  the  plane.  Assume  without  loss  of  generality  that  the  sensor  is  mounted 
on  the  robot  at  the  reference  point.  Then  we  must  simply  replace  the  general¬ 
ized  configuration  space  obstacles  in  the  construction  o{  Us  and  Ur  hy  the  set  of 
generalized  real  space  obstacles,  B.  B  represents  a  variational  family  of  real-space 
obstacles.  Intuitively,  O  represents  the  B  “grown”  by  the  shape  of  the  robot.  That 
is,  O  is  constructed  by  convolving  each  generalized  real-space  obstacle  with  the 
robot  geometry. 

More  precisely,  let  B^(a)  denote  a  particular  shape  of  an  obstacle  in  the  envi¬ 
ronment  for  a  in  J.  0  indexes  the  set  of  all  such  obstacles.  Then 

Now,  assume  for  simplicity  that  the  shape  A  of  the  robot  is  exactly  known.  If  © 
denotes  convolution  with  the  reflection  as  in  [Lozano-Perez],  then 

O  =  |{(H^(a)e  A,Q)}^gj|^. 

Thus  of  course,  for  point  robots,  O  and  B  are  identical. 

*  13.2.4  Rotations 

We  now  incorporate  rotations  into  the  planning  framework  for  active  sensing. 
First,  in  the  construction  of  the  physical  motion  universe  Um  (1),  we  construct  Q 
as  9?^  X  X  J,  using  the  Euclidean  group  on  the  plane  for  C.  The  generalized 
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configiiration  space  obstacles  O  are  constructed  in  the  usual  way  for  rotations  and 
translations.  Next,  we  will  leave  ^  as  3?^  x  /  in  constructing  the  sensing  2ind  return 
universes,  (2)  and  (3).  The  obstacles  in  the  sensing  and  return  universes  are  the 
generalized  real-space  obstacles  B  as  described  above.  Again,  the  generalized  motion 
Move{<f>)  moves  only  in  C,  without  changing  the  position  in  J.  However,  note  that 
Move{4>)  is  restricted  to  be  a  pure  translation  in  Us,  terminating  on  some  real  space 
obstacle  in  B.  This  is  am  important  difference.  The  physical  motion  commands  can 
move  with  three  degrees  of  freedom  in  C  within  Um',  however,  the  sensing  command 
moves  as  a  pure  translation  in  Us-  Furthermore,  it  moves  in  generalized  real-space, 
amidst  generalized  real-space  obstacles,  whereais  the  physical  motion  commands  in 
U M  naove  in  generalized  configuration  space  amidst  generalized  configuration  space 
obstacles. 

Some  technical  changes  are  required  in  the  Move(S)  and  Move{R)  operators. 
When  Move{S)  is  executed  from  actual  configuration  (x,  y,  d)  in  the  physical  mo¬ 
tion  universe  Um,  the  forward  projection  consists  of  the  point  (x,y)  in  the  sensing 
universe  Us-  The  original  actual  position  is  stored  in  a  variable  (xo,yo,^o)-  This 
variable  is  used  by  the  pure  position  control  command  Moveto  in  the  perfect  rettim 
universe  Ur.  This  is  easy  to  formalize  by  representing  the  generalized  configuration 
space  in  (4)  as  ^  X  3,  where  Um,  Us,  and  Ur  are  identified  with  the  subspaces 
^x{0},^x{l},  etc.  We  then  view  the  motions  between  universes  hs  a  combined 
projection  and  shift.  For  example, 

Mave{S)  -  Um  -*Us  CQ  x3 
{(x,y,9),Q)  •-»  ((x,y),l). 

Move{R)  may  be  formaUzed  similarly  as  a  combined  hfting  and  shift. 

This  completes  the  reduction  of  EDR  planning  with  active  sensing  to  EDR 
planning  in  a  three-fold  generalized  configuration  space. 

13.3.  Discussion 


In  this  chapter,  we  described  a  reduction  of  EDR  planning  with  active  sensing  to 
(ordinary)  EDR  planning  in  a  larger  generalized  configuration  space  that  represents 
both  motion  commands  and  sensing  commands  as  “generalized  motions.”  The  re¬ 
duction  involves  defining  a  kind  of  “equivalence”  between  sensing  and  motion,  which 
permits  an  EDR  planner  to  treat  sensing  and  motion  commands  “uniformly.”  These 
generalized  motions  can  be  represented  in  a  “threefold  cover”  of  generalized  con¬ 
figuration  space.  The  equivalence  defined  relies  on  the  similarity  between  visibility 
analysis  and  generalized  djimper  motions. With  our  tools  for  planning  with  model 
error — specifically,  the  generalized  configuration  space  formalism — it  was  possible  to 

‘‘This  similarity  was  exploited  extensively  by  [Buckley]. 
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give  a  precise  characterization  of  what  it  means  to  plan  with  active  sensing,  and  to 
derive  a  formal  method  for  constructing  these  plans.  The  generalized  configuration 
space  representation  was  critical  not  only  in  representing  the  uncertain  environ¬ 
ment,  but  adso  in  defining  a  planning  model  for  active  sensing.  It  is  interesting  to 
note  that  while  generalized  configuration  space  was  originally  envisioned  exclusively 
as  a  framework  for  representing  geometric  model  uncertainty  [D],  it  appears  to  have 
broader  applicability  in  planning  pushing  operations  and  active  sensing.  In  EDR 
planning  with  active  sensing,  generalized  configuration  space  is  particularly  useful 
in  developing  a  systematic  model  of  the  error  in  absolute  position  vs.  the  error  in 
the  map  of  the  world:  the  first  is  position  error  in  C,  the  second  is  position  error 
in  J.  Both  may  be  reduced  through  an  appropriate  choice  of  physical  motions  or 
active  sensing.  However,  error  in  C  can  grow  with  physical  motion,  while  error 
in  J  cannot.®  Furthermore,  active  sensing  can  only  reduce  the  error  in  C  and  J; 
it  can  never  increase  it.  When  viewed  in  this  manner,  it  is  not  at  all  clear  that 
there  should  be  any  unifying  concept  for  physical  motion  and  active  sensing!  It  is 
even  more  surprising  that  the  unifying  tool  should  emerge  as  our  familiar  friend, 
generalized  configuration  space. 

This  reduction  yields  an  effective  technique  for  planning  motions  and  active 
sensing  for  a  mobile  robot  in  a  partially  known  environment.  This  technique  pro¬ 
vides  a  principled  approach  to  motion  planning  with  active  sensing.  It  shows  how  to 
incorporate  a  more  fine-grained  model  of  sensing  into  the  EDR  planning  framework. 
As  a  corollary,  of  course,  we  obtain  a  method  for  plaiming  guaranteed  strategies 
with  active  sensing. 

Much  work  remains  to  be  done.  In  particular,  the  EDR  framework  for  ac¬ 
tive  sensing  is  still  fairly  theoretical.  Mobile  robot  environments  are  often  highly 
unstructured  [Brooks,  85],  and  representing  this  geometric  uncertainty  using  gener- 
adized  configuration  space  presents  a  non-trivial  problem.  Furthermore,  it  may  be 
impractical  to  model  more  general  vision  or  sonar  sensors  without  further  enhance¬ 
ments  to  the  EDR  theory.  More  study  is  required;  hopefully  this  theory  of  EDR 
planning  with  active  sensing  can  provide  a  starting  point. 


®unle8S  pushing  is  allowed. 


V.  Implementation,  Computational  Issues 


In  this  chapter,  we  describe  the  LIMITED  plane-sweep  algorithm.  We  then 
turn  to  the  problem  of  generating  motion  strategies.  LIMITED  has  a  crisp  algo¬ 
rithm  for  verifying  EDR  strategies,  but  to  generate  a  strategy,  it  must  quantize 
the  space  of  commanded  motions  and  enumerate  motion  strategies  exhaustively. 
How  can  motion  plans  be  generated  without  exhaustive  quantization  of  the  space 
of  commanded  directions?  To  this  end  we  introduce  the  non- directional  backpro- 
jection.  It  allows  us  to  devise  exact  algorithms  for  planning  guaranteed  strategies, 
given  certain  restrictions.  We  also  axidress  generalizing  such  algorithms  to  planning 
multi-step  strategies,  and  to  generating  EDR  strategies.  While  the  motion  planning 
with  imcertainty  is  known  to  be  hard  for  exponential  time  [Canny  and  Reif],  we  Eire 
able  to  identify  certain  interesting  subclasses  of  planning  problems  which  are  easier 
(polynomial  or  single-exponential  time).  These  techniques  for  generating  multi-step 
strategies  will  hopefully  be  useful  in  EDR  planning  as  well. 
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14.  Implementation,  Computational  Issues 


14.1.  Comments  on  the  Plane  Sweep  Algorithm 

Given  a  2D  slice  of  generalized  configuration  space,  LIMITED  employs  a  plane- 
sweep  algorithm  for  computing  unions,  intersections,  and  projections.  (By  projec¬ 
tions  we  mean  forward  projections,  backprojections,  and  weak-backprojections  in 
that  slice).  The  algorithm  uses  exact  (rational)  arithmetic,  and  computes  unions 
in  0((n  -f  c)  logn)  time,  2ind  projections  in  0(n  log n)  time.^  The  design  and  im¬ 
plementation  of  the  2D  plane-sweep  module  is  joint  work  with  John  Canny;  the 
algorithm  is  based  on  (Neivergelt  and  Preparata]  (who  give  a  union  algorithm)  and 
related  to  [Erdmann]  (who  implemented  an  O(n^)  backprojection  algorithm,  and 
suggested  an  improved  O(nlogn)  version).  In  this  section  we  briefly  discuss  some 
details  of  the  algorithm.  A  full  listing  of  the  ZetaLisp  code  for  the  plane-sweep 
algorithm  miming  on  a  Symbolics  3600  is  provided  in  an  appendix.  In  LIMITED 
there  are,  of  course,  many  software  layers  built  on  top  of  the  sweep  algorithm  for 
quasi-static  analysis,  EDR  planning,  propagation  across  sUces,  distinguishability, 
and  so  on.  In  EDR  planning,  we  essentially  reduce  the  problem  of  EDR  verification 
to  deciding  certain  set-relations.  The  basic  sets  are  projection  regions.  Both  the 
projections  and  the  set  operations  are  computed  by  calls  to  the  sweep  algorithm. 
The  design  and  implementation  of  a  robust  geometric  engine  is  a  formidable  task. 
In  this  section  we  share  some  of  our  experiences. 

We  do  not  go  on  at  great  length  about  the  details  of  the  algorithm  because,  first 
of  all,  it  is  fairly  complicated,  and  second,  from  a  complexity- theoretic  viewpoint, 
the  result  does  not  improve  known  boimds  by  much.  Readers  interested  in  the 
details  of  the  algorithm  will  find  them  in  the  appendix. 

14.1.1  The  Basic  Idea 

We  now  sketch  the  classiczd  plane  sweep  approach  at  a  high  level.  In  plane 
sweep  algorithms,  the  vertices  of  the  input  edges  are  sorted  on  planar  lexicographic 
x-y  order,  eg.,  lower  left  to  upper  right.  This  is  accomphshed  using  an  AVL  tree. 
A  Une  is  swept  across  the  plane  in  this  order.  The  algorithm  keeps  track  of  the 
polygonal  regions  swept  across  by  maintaining  an  ordered  queue  of  intervals  on 
the  sweep  line.  This  queue  is  also  maintained  using  a  (different)  AVL  tree.  Each 
interval  along  the  sweep  fine  has  an  associated  “color.”  The  color  is  an  integer;  0  for 
free  space,  1  for  a  region  inside  one  input  polygon,  2  for  a  region  inside  two  input 
polygons,  etc.  The  boundEuies  of  the  intervals  grow  or  shrink  with  the  sweep  in  a 

*  Where  n  is  the  number  of  vertices  in  the  slice,  and  c  is  the  number  of  intersections. 


known  way:  their  change  is  given  by  the  line  equation  of  their  endpoints.  These 
line  equations  axe  teiken  from  the  line  equations  of  the  input  edges. 

An  “event”  occurs  when  a  new  vertex  is  encountered  in  the  sweep.  Such  an 
event  edfects  one  or  more  of  the  intervals  in  the  intervzJ  queue  on  the  sweep  line. 
For  exeimple,  in  a  “closing”  event,  the  “end”  of  a  polygon  heis  been  encountered  (it 
has  closed  up),  so  all  of  the  polygon  lies  to  the  left  of  the  sweep  line.  In  this  czise, 
the  interval  associated  with  the  polygon  is  deleted,  the  two  surroimding  intervals 
axe  merged,  and  the  polygon  loop  is  placed  on  the  output  queue.  Other  events 
include  “start”  (staxt  a  new  polygon)  and  “crossing”,  when  the  line  equations  of 
three  adjacent  intervals  intersect.  In  a  crossing  event,  two  line  segments  intersect 
and  their  associated  interval  boundaries  must  be  merged. 

In  developing  a  sweep  algorithm  for  projections,  we  proceed  as  follows.  Con¬ 
sider  the  forward  projection.  We  introduce  two  new  colors,  the  projection  color 
and  the  start  region  color.  The  sweep  proceeds  in  the  direction  of  the  commanded 
motion.  When  the  sweep  encounters  the  staxt  region,  then  intervals  of  color  start 
region  axe  inserted  into  the  interval  queue  adong  the  sweep  line.  When  these  inter¬ 
vals  close,  then  intervals  of  color  projection  must  be  queued.  The  line  equations  of 
the  free-space  endpoints  of  these  projection  intervals  axe  paxallel  to  the  sides  of  the 
commanded  velocity  imcertainty  cone.  This  occurs  when  the  projection  intervals 
border  free-space  intervals. 

When  an  interval  of  type  projection  crosses  an  interval  of  type  obstacle  (color 
>  1),  then  either  (1)  it  may  be  closed  off,  (2)  the  obstacle  edge  boundary  may  be 
teiken  as  the  projection  region  boundary  by  updating  the  line  equation  of  the  projec¬ 
tion  interval’s  endpoints,  or  (3)  depending  on  sliding  behavior,  a  new  “degenerate” 
interval,  with  no  width,  sliding  along  the  edge  may  be  queued.  Whether  the  mo¬ 
tion  can  slide  on  an  edge  e  is  determined  by  intersecting  the  reflected  ( “negative” ) 
velocity  cone  with  the  friction  cone  on  edge  e.  In  ceise  (3),  the  line  equations  of 
degenerate  interval’s  endpoints  axe  copied  from  e. 

When  the  sweep  is  complete,  the  output  is  an  arrangement  of  polygons  with 
different  colors,  including  the  projection  and  the  steirt  region  colors.  The  forward 
projection  is  simply  all  polygons  with  color  projection  or  start  region.  This  algo¬ 
rithm  is  correct  given  the  following  assumption: 


Correctness  Criterion:  The  plane  sweep  algorithm  is  correct  when  (a)  the  friction 
cone  is  larger  than  the  commanded  velocity  uncertainty  cone  and  (b )  there  is  a 
bounding  box  around  the  input  environment. 


This  criterion  is  necessary,  because  the  sweep  algorithm  is  monotonic;  hence 
to  be  correct,  we  must  ensure  that  motions  are  also  monotonic  and  cannot  back  up 
on  surfaces. 
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By  introducing  a  goal  color,  backprojections  and  weak  baw:kprojections  axe  com¬ 
puted  analogously,  sweeping  in  the  opposite  direction  to  the  commanded  motion. 
The  weak  backprojection  is  actually  a  conservative  (under)-approximation,  since 
it  does  not  take  into  account  weak  backsliding  [Buckley].  This  is  because  weak 
backsliding  is  non-monotonic  and  so  a  sweep  algorithm  will  not  suffice.  Actually, 
our  plane  sweep  algorithm  can  only  sweep  in  one  fixed  direction;  hence  we  rotate 
the  environment  first  so  that  the  sweep  axis  coincides  with  the  commanded  motion 
direction,  and  then  rotate  the  projections  back  to  the  canonical  orientation. 

For  details  of  the  sweep  algorithm,  please  see  the  appendix. 


14.1.2  Contrast  with  Previous  Algorithms 

We  now  compare  our  algorithm,  which  we  call  Sweep,  with  previous  work. 


1.  Sweep  combines  the  ability  to  compute  set  operations  and  projections  in  one 
sweep. 

2.  The  plane  sweep  algorithm  of  [Neivergelt  and  Preparata]  for  computing  set 
operations  on  polygonal  regions  assumes  general  position.  Sweep  does  not. 

3.  Note  that  [Erdmann]  described  the  first  backprojection  algorithm  in  the  plane. 
He  also  described  slice  algorithms  for  2D  with  rotations.  [Buckley]  described  non¬ 
slice  backprojection  and  forward  projection  algorithms  in  3D  with  no  rotations.  All 
these  algorithms  have  been  implemented. 

4.  The  algorithm  of  [Erdmann]  czm  compute  the  backprojection  of  a  single  edge  in 
time  O(nlogn).  In  Sweep,  the  goal  region  can  be  an  arbitrary  polygon.  Similarly, 
in  Sweep,  the  start  region  for  forward  projections  can  be  an  arbitrary  polygon. 

5.  Sweep  is  implemented  xising  exact  (rational)  arithmetic. 

6.  Sweep  can  compute  forward  and  weak  backprojections  as  well  as  strong  back- 
projections. 


14.2.  Non-Directional  Backprojections 


14.2.1  Intuition 

Limited  is  a  generate-and-test  planner.  We  have  elaborated  the  “test”  portion — 
verification  of  EDR  strategies  as  decision  problems  about  projection  sets.  Now  it 
is  time  to  take  a  more  sophisticated  look  at  the  “generation”  problem.  How  can 
motion  strategies  be  generated  without  exhaustive  quantization  and  search? 


A  significant  weaJcness  of  LIMITED  is  its  method  for  generating  commanded 
motions.  It  simply  quantizes  the  space  of  all  motions.  Thus  to  generate  two- 
step  plans  di  *  $2  Limited  must  quantize  the  space  of  motions  6i  to  generate  the 
first  motion,  and  then  quantize  the  space  of  motions  for  62-  Essentially,  LIMITED 
implements  an  existential  theory;  the  planner  can  verify  a  strategy  but  the  strategy 
must  be  “guessed”  by  some  oracle,  or  by  exhaiostive  search.  This  is  theoretically 
unsatisfying,  as  well  as  impractical.  We  now  address  this  problem.  In  particular,  we 
provide  a  method  for  generating  two-step  plans  9i  *62  which  only  requires  “guessing” 
9i .  That  is,  once  9i  is  provided,  92  can  be  generated. 

To  this  end  we  define  a  combinatorial  object  called  the  non- directional  backpro- 
jection,  and  give  a  critical  slice  algorithm  for  constructing  it.  The  non-directional 
backprojection  may  be  used  to  represent,  in  a  sense,  “all  possible  backprojections” 
of  a  fixed  goal.  We  intend  to  use  it  to  generate  motion  strategies. 

[LMT]  first  defined  non-directional  preimages.  [Erdmann]  defined  the  non- 
directional  backprojection  as  the  union  of  all  backprojections  in  the  plane: 
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We  will  use  a  different  definition.  However,  it  is  in  the  same  spirit  as  [LMT,E], 
and  so  we  will  employ  the  same  name.  We  must  point  out,  however,  that  both 
M.  Erdmann  and  R.  Brost  have  considered^  a  similar  construction  for  generating 
commanded  velocities,  and  also  thought  about  a  critical  slice  approach  to  computing 
it. 

Our  definition  exploits  generalized  configuration  space.  Consider  the  following 
argument. 

1.  Supi>ose  we  have  a  planar  polygonal  environment  with  no  model  error.  In 
generating  motion  strategies,  we  do  not  know  which  way  to  point  the  robot — 
that  is,  we  do  not  know  which  way  to  command  the  motion.  Thus  in  some  sense, 
there  is  “uncertainty”  in  “which  way  to  go.”  This  “vmcertainty”  is  the  variable 
9.  Thus  we  have  a  kind  of  three-dimensional  planning  problem,  with  degrees  of 
freedom  i,  y,  9.  As  the  reader  may  expect,  we  intend  to  map  this  imcertainty 
in  “which  way  to  go”  into  our  familiar  friend,  generalized  configuration  space. 

2.  Now,  consider  a  problem  which  is  in  some  sense  dual  to  generating  motion 
strategies.  In  this  problem,  we  only  consider  one  commanded  motion  in  a  fixed 
direction  uj.  However,  there  is  total  uncertainty  in  the  orientation  of  the  entire 
environment.  We  may  represent  this  uncertainty  by  a  variable  9  also. 

Clearly,  both  problems  (1)  and  (2)  can  be  represented  in  an  generalized  con¬ 
figuration  space  where  x  and  y  are  the  degrees  of  motion  freedom,  and  9  i.s  “model 


[Personal  communication].  I  am  grateful  to  M.  Erdmann  for  pointing  out  the  similarity 
of  the  construction. 
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error.”  Here  is  the  difference,  however.  In  (2),  9  is  universally  quantified:  that  is, 
we  are  required  to  ensure  that  a  motion  strategy  succeeds  for  all  6.  In  (1),  however, 
6  is  existentially  quantified.  We  merely  need  one  9  to  find  a  commanded  motion. 

The  precise  analogue  of  (1)  is  a  problem  like  (2)  in  which  we  get  to  choose 
the  orientation  of  the  environment  such  that  the  Vg,  the  fixed  commanded  motion 
imder  consideration,  will  guarantee  reaching  G. 


14.2.2  Computing  the  Non-Directional  Backprojection 


We  now  make  the  intuitive  argument  more  precise.  Let  J  be  the  space  of  all 
commanded  motions,  so  that  J  is  exactly  the  circle,  S^.  We  write  9  €  J  for  a 
commanded  motion  direction. 


Definition:  £ei  G  be  a  goal  amidst  polygonal  obstacles  in  the  plane. 
Directional  Backprojection  B(G)  of  G  is  a  set  in  x  J, 


The  Non- 


S(G)=U(s.(G)x{«}). 


(1) 


Now,  recall  the  critical  slice  algorithms  of  sec.  6.4.  These  algorithms  com¬ 
puted  3D  directional  backprojections  in  a  three  dimensional  generalized  configura¬ 
tion  space,  3?^  X  They  operate  by  determining  critical  orientations  at  which  the 
topology  of  backprojection  slices  change. 

B{G)  is  also  a  3D  backprojection-like  region.  We  can  develop  critical  slice 
algorithms  for  computing  B{G)  also.  They  will  work  by  finding  edl  values  of  9  at 
which  the  topology  of  Bg{G)  can  change.  Then  the  algorithm  takes  slices  at  these 
critical  ^’s  and  at  an  intermediate  non-critical  ^’s  between  each  pair  of  adjacent 
critical  values. 

Now,  B(G)  is  boimded  by  developable  algebraic  surfaces.  These  surfaces  are 
of  two  types,  obstacle  surfaces,  and  free-space  surfaces.  The  obstacle  surfaces  are 
liftings  into  x  J  of  the  obstacle  edges  in  3?^.  The  free-space  surfaces  su'e  swept 
out  by  free-space  edges  of  5«(G)  ^ls  they  rotate  with  9.  The  manner  in  which  the 
boimding  algebraic  svirfaces  of  B{G)  sweep  between  slices  is  completely  known — 
the  obstacle  edges  stay  fixed,  while  the  free-space  edges  rotate  with  9,  remaining 
parallel  with  edges  of  the  velocity  cone.  Now,  each  free-space  edge  is  anchored  at  an 
obstacle  vertex  cobounding  a  possible  sticking  edge.  As  9  varies,  the  free-space  edge 
rotates  about  that  vertex.  Clearly,  as  9  varies,  the  topology  of  B${G)  can  change 
if  the  free  space  edge  contacts  an  obstacle  vertex.  When  this  happens,  there  is  an 
edge  connecting  two  obstacle  vertices  which  is  parallel  to  an  edge  of  the  commanded 
velocity  cone.  Next,  we  note  that  any  such  edge  lies  in  the  visibility  graph  of  the 
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planax  input  environment.  The  visibility  graph  may  be  computed  in  time  O(n^). 
This  gives  us  the  following  lemma,  which  gives  an  upper  boimd  on  the  number  of 
critical  values  of  9.  Here  is  the  intuition  behind  the  lemma: 


Consider  a  free-space  edge  ei(9)  of  B$(G).  ei(9)  lies  in  the  infinite  half-ray 
ri(9)  which  extends  from  c,(d)’s  anchor  vertex.  We  call  ri(9)  a  constraint  ray, 
it  is  parallel  to  an  edge  of  B^dvg)-  There  are  0(n)  constraint  rays  in  each 
backprojection  slice  Bs^G).  ri($)  rotates  with  9,  and  it  can  intersect  0(n) 
obstacle  edges  as  9  sweeps  along.  Now,  how  many  other  constraint  rays  of 
the  form  rj(9)  can  ri(9)  intersect  as  it  rotates?  Note  that  all  constraint  rays 
{  }  niove  “with”  ri(9),  and  are  either  parallel  to  it,  or  else  intersect  it 

ailways.  Therefore  how  ri(9)  can  intersect  these  other  constraint  rays  as  9 
sweeps  is  aiso  0(n). 

We  assume  that  the  input  polygons  represent  configuration  space  obstacles.® 
We  use  the  boundary  operator  d  to  denote  the  topological  boundary. 


Lemma.  Given  a  goal  G  of  constant  size  and  an  arrangement  of  input  polygons  V  of 
size  0(n),  there  are  O(n^)  critical  values  of  9  in  the  non- directional  backprojection 
BiG). 

Proof.  Let  Bec(wJ)  denote  the  control  velocity  uncertainty  cone  about  a  commanded 
velocity  Vg.  We  think  of  Bec(vg)  as  rotating  with  9.  The  topology  of  BaiG)  can 
change  when  any  of  the  following  occur: 


A.  An  edge  of  H«c(u5)  becomes  parallel  to  an  edge  in  the  visibility  graph  of  V. 
Such  values  of  9  are  called  vgraph-cntical. 

B.  ^  is  a  j/idm^-critical  value  (see  sec.  6.4),  where  the  determination  of  sliding 
vs.  sticking  behavior  on  an  edge  can  change.  Sliding-critical  values  occur  when 
an  edge  of  Bedvg)  becomes  parallel  to  the  edge  of  a  friction  cone  on  some 
configuration  space  edge. 

C.  Let  edO)  and  ej(9)  be  free-space  edges  of  Be(G).  They  rotate  with  9  about 
their  anchor  vertices.  Let  Pij(9)  denote  their  intersection;  it  is  a  free-space 
vertex  of  the  backprojection.  Then  9  is  vertex-critical  when  Pij(9)  G  dBa(G) 
and  Pij{9)  intersects  some  obstacle  edge. 

Now,  there  are  0{rd)  edges  in  the  visibility  graph  of  V.  In  sec.  6.4  we  showed 
that  there  are  0{n)  sliding-critical  values.  Only  sliding-critical  values  can  introduce 
additional  constraint  rays. 

Now,  since  there  are  0{n)  constraint  rays  in  each  slice,  it  would  appear  a 
priori  that  there  could  be  potentially  0{n^)  pij{9ys.  Note,  however,  that  each 

®See  sec.  6.4  for  the  complexity  where  the  input  is  given  in  real  space  obstacles. 


free-space  vertex  Pij(B)  of  the  baickprojection  can  be  identified  with  exactly  one 
constraint  ray,  say  the  “left”  one,  ri(ff).  Hence  we  see  that  there  are  merely  0(n) 
Each  moves  in  a  circle.  Observe  that  in  effect,  each  free-space  vertex  of 
the  backprojection  moves  with  d  in  a  piecewise-circular,  possibly  disconnected  locus. 
Consider  the  discontinuities  in  the  locus  caused  by  type  (A)  or  (B)  critical  values. 
In  between  discontinuities,  each  circular  arc  in  the  locus  can  intersect  only  a  fixed 
mber  of  obstacle  edges.  In  particular,  the  arc  cannot  intersect  n  obstacle  edges 
without  “using  up”  more  type  (A)  or  (B)  critical  values.  Hence,  there  are  O(n^) 
vertex -critical  values  of  0. 

Next  we  observe  that  the  boimds  for  (A)  (B)  and  (C)  are  additive.  In  particular: 
the  bounds  on  vertex-critical  and  vgraph-critical  values  apply  to  all  possible  free- 
space  edges;  hence  the  vgraph-critical  and  vertex-critical  values  do  not  interact  and 
their  complexities  do  not  multiply.  Similarly,  the  sliding-critical  bounds  cover  all 
possible  ways  that  a  constraint  ray  can  be  added  or  deleted  from  the  backprojection 
boundary  as  ^  changes.  Hence  this  boimd  is  tilso  additive.  Thus  we  obtain  the  O(n^) 
upper  bound.  □ 

Corollary;  There  exists  a  representation  of  size  0(n®)  for  the  non- directional  back- 
projection  B{G). 

Proof:  Take  O(n^)  slices  at  critical  values.  Compute  a  backprojection  slice  Bs{G) 
of  size  0(n)  at  each  of  the  critical  values  of  9.  □ 

Comments:  This  upper  bound  means  that  O(n^)  slices  are  required  for  a  critical 
shce  representation  of  B{G).  However,  as  in  sec.  6.4,  it  seems  that  this  upper  bound 
will  almost  never  be  attained  in  practice.  In  practice  we  will  consider  only  small 
ranges  of  9.  For  exaunple,  for  a  peg-in-hole  strategy,  we  would  probably  only  consider 
directions  in  the  lower  (downward)  half-plane.  While  these  arguments  do  not  affect 
the  worst-case  complexity,  they  do  suggest  that  in  practice  the  number  of  critical  9 
values  may  he  smaller  than  O(n^). 

We  can  now  address  the  complexity  of  computing  B{G).  By  this  we  mean, 
what  is  the  complexity  of  computing  a  precise,  combinatorial  description  of  H(G). 
The  output  representation  is  a  finite  ordered  set  of  alternating  critical  and  non- 
critical  slices  {  Be^  (G),  Be„^  (G), . . .  },  along  with  an  algebraic  description  of  how 
the  free-space  edges  of  the  backprojection  change  between  slices.  (For  a  free-space 
edge,  this  is  completely  specified  by  the  anchor  vertex  and  an  interval  of  9  for  which 
the  surface  bounds  B{G)). 

As  above,  let  'P  be  an  arrangement  of  input  polygons  representing  configuration 
space  obstacles. 


Theorem:  Given  a  goal  G  of  constant  size  and  an  arrangement  of  input  polygons 
V  of  size  0(n),  a  representation  of  size  0(n*)  for  the  non- directional  backprojection 
B(G)  can  be  computed  in  time  O(n‘‘logn). 

Proof.  First,  we  compute  the  critical  values  of  6.  Sliding-critic^ll  values  can  be  com¬ 
puted  in  linear  time.  Vgraph-critical  values  can  be  computed  in  time  O(n^logn). 
While  it  may  be  possible  to  compute  the  vertex-critical  values  in  quadratic  time, 
we  give  the  following  simple  O(n^)  algorithm:  Intersect  all  constraint  rays  to  ob¬ 
tain  0{n^)  points  Pij{0).  Each  of  these  points  is  a  possible  free-space  vertex  of  the 
backprojection,  and  each  moves  in  a  circle  with  9.  Intersect  these  circles  with  the  ob¬ 
stacle  edges  to  obtain  0{n^)  possible  critical  values  of  9.  The  actual  vertex-critical 
values  will  be  contained  in  this  set. 

Compute  0(n®)  slices  B0(G),  at  each  the  possibly-critical  value  9,  using  Sweep. 
Sweep  computes  a  2D  backprojection  slice  in  time  0(n  log  n),  and  the  output  has 
size  0(n).  □ 

Some  comments  are  in  order.  First,  our  algorithm  is  naive,  in  that  each  back- 
projection  shce  is  recomputed  from  scratch.  In  fact,  this  extra  work  is  unnecessary. 
At  a  critical  value  of  9,  very  few  aspects  of  the  topology  of  the  backprojection  will 
change.  That  is,  typically,  only  one  or  two  free-space  edges  will  be  introduced, 
or  disappear,  or  change  at  any  critical  value.  We  can  make  this  notion  precise  as 
follows.  If  ^  is  a  generic  singularity,  then  exactly  one  edge  or  vertex  of  B0(G)  will 
appear  or  disappear  there.  Hence,  for  example,  we  can  ensure  that  all  critical  val¬ 
ues  are  generic  singularities  with  probability  one  by  subjecting  the  input  to  small 
rational  perturbations. 

Suppose  that  a  backprojection  has  been  computed  in  a  critical  slice  at  9.  Then 
to  compute  a  backprojection  in  a  nearby  non-critical  slice  at  9  e,  we  merely  need 
to  update  the  portion  of  the  backprojection  boundary  which  was  critical  at  9.  This 
requires  only  constant  work:  only  one  edge  or  vertex  must  be  changed  to  derive 
a  backprojection  in  the  new  slice!  The  new  slice,  furthermore,  need  not  be  copied 
in  entirety.  Instead,  the  representation  for  the  new  slice  can  simply  indicate  how 
it  has  changed  from  the  old  slice.  It  is  reasonable  to  speculate  that  this  technique 
would  yield  an  algorithm  of  time  and  space  complexity  0{n^  logn)  for  computing 
B{G).  (The  log  factor  arises  from  the  necessity  of  sorting  the  critical  values). 


14.2.3  The  Non-Directional  Forward  Projection 

The  “dual”  to  the  non-directional  backprojection  is  the  non-directional  forward 
projection: 


Definition:  Let  R  be  a  atari  region  amidat  polygonal  obataclea  in  the  plane.  The 
Non-Directional  Forward  Projection  F{R)  of  R  ia  a  aet  in  3?^  x  J, 


FiR) 


=y( 


n(i2)x{^} 


As  a  corollary  to  our  bounds  on  the  complexity  of  the  non-directional  backpro- 
jection,  we  obtain  the  following  theorem  which  may  be  derived  mutatia  mutandia: 

Theorem:  Given  a  atari  region  R  of  conatant  aize  and  an  arrangement  of  input 
obatacle  polygona  V  of  aize  0(n},  let  F(R)  be  the  non-directional  forward-projection 
of  R.  Then 

a.  there  are  O(n^)  critical  valuea  of  9  for  F{R); 

there  exiaia  a  repreaentaiion  of  aize  0(n*)  for  F{R); 
c  a  repreaentaiion  of  aize  0{n*)  for  F{R)  can  be  computed  in  time  0{n*\ogn). 
□ 

We  will  need  the  following  corollary  later; 

Corollary:  For  a  conatant- aized  atari  region  R  and  goal  region  G,  amidat  an  ar¬ 
rangement  of  input  obatacle  polygona  V  of  aize  0{n),  the  non-directional  forward 
projection  F(R)  and  non-directional  backprojeciion  B{G)  have  repreaentationa  aa 
polynomial- aized  formulae  in  the  language  of  aemi-algebraic  fa. a.)  aeta.  Further¬ 
more,  iheae  formulae  are  quantifier- free. 

Proof:  We  can  represent  the  non-directional  forward  projection  (resp.,  backprojec- 
tion)  at  a  polynomial  (in  n)  number  of  critical  values  {di,...,9i}  via  the  formula 

I 

A=/\(9  =  9.  (x,y)eFe,(R)). 

1=1 

Let  two  adjacent  critical  values  be  and  In  between  adjacent  criti¬ 

cal  values  of  9,  the  non-directional  projection  is  bounded  by  a  fixed'*  set  of  0(n) 
developable  algebraic  surfaces.  That  is,  when  9  is  between  and  the  non- 

directional  projection  is  the  intersection  of  some  fixed  set  of  0(n)  algebraic  half¬ 
spaces.  These  half  spaces  are  represented  by  algebraic  inequalities,  {  gij(x,y,  9)  < 
0 }  where  each  gij  is  a  polynomial.  The  form  of  the  g,j  is  discussed  in  14.2.2.  We 
define  the  predicate 


C,  =  /\(9,j(^,y,^)  <  0), 


^i.e.,  fixed  between  0,'"'"  and 
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where  rrii  is  0(n).  We  construct  the  non-directional  projection  as  a  s.a.  set  in  a 
case  statement, 


I 


AA/\(^€(C“.^r’')  Ci). 


□ 


14.3.  Generating  Multi-Step  Strategies  using  the  Non-Directional 
Backprojection 


We  now  describe  how  to  employ  the  non-directional  backprojection  B(G)  to 
generate  two-step  strategies  with  “less  quantization.”  More  precisely:  while  LIMITED 
is  required  to  hypothesize  both  the  first  motion  0i  and  the  second  motion  $2 ,  we  can 
show  how,  given  di ,  82  may  be  computed.  Hence  only  $1  need  be  guessed  through 
exhaustive  quantization,  and  $2  can  be  computed  deterministically. 

Let  C  be  the  configuration  space  3?^,  and  J  the  space  of  commanded  motions 
5^  as  above.  Define  the  projection  map 


Tcj  :  C  y.  J  J 
(x,y,B)  9. 

Now,  algorithm  Semi-Plan^  below,  takes  a  first  motion  ^1,  the  goal  G,  the 
start  region  R  and  the  set  V  of  input  polygons  representing  the  arrangement  of 
configuration  space  obstacles.  It  computes  the  set  T  of  all  commanded  motions  62 
such  that  9i  *  62  reaches  G. 


Algorithm  Semi- Plan 


1. 

2. 


pushg^  F,g^{R). 
R\  ^  push^j  X  7, 


3. 

4. 


T^J-T,j[Ri  -B{G) 

Return  any  62  €  T. 


To  see  that  Semi-Plan  is  correct,  we  simply  observe  that  J  is  the  set  of  all 
commanded  motions  $2,  and  that 


nj{R^-B{G) 


(2) 


is  simply  the  projection  onto  .7  of  where  the  push-forward  of  9i  lies  outside  the 
non-directional  backprojection.  Choosing  any  $2  in  the  complement  of  (2)  results 
in  a  two  step  motion  that  is  guaranteed  to  reach  G. 
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c. 


/.'.N 


a 


Algorithm  Semi-Plan  has  severzd  advantages  over  exhaustive  quantization  of 
both  di  and  62  spaces  of  directions.  First,  it  requires  less  quantization.  Second,  it 
provides  all  62  such  that  $i  *  62  reached  G,  instead  of  just  one.  Third,  the  Edgorithm 
is  crisper,  in  that  it  exploits  the  structure  of  the  non- directional  backprojection;  the 
algorithm  is  not  blind.  Finally,  it  is  possible  to  give  precise  analyses  of  Semi-Plan's 
combinatorial  complexity,  as  above.  Clearly,  the  complexity  of  computing  B{G) 
will  dominate. 

14.4.  Comments  and  Issues 

Semi-Plan  represents  a  theoretical  algorithm.  It  has  not  been  implemented  in 
Limited.  It  was  described  here  to  give  some  characterization  for  boimds  on  comput¬ 
ing  multi-step  strategies.  In  particular,  it  gives  a  precise,  combinatorial  description 
for  the  3D  non- directional  backprojection  B(G)  for  a  planar  polygonal  configuration 
space  environment.  Semi-Plan  directly  addresses  the  question  of  planning  two-step 
strategies.  The  critical  slice  method  attempts  to  put  the  directional  backprojection 
techniques  used  in  LIMITED  on  a  firm  mathematical  footing.  It  gives  a  principled 
way  to  choose  motion  $2  given  0i,  a  bound  on  how  many  slices  are  required,  and 
an  algorithm  which  does  not  have  to  exhaustively  enumerate  the  possible  second 
motions  62. 

Note  that  if  we  were  merely  interested  in  one-step  strategies,  then  a  variation  on 
Semi-Plan  provides  a  way  to  compute  the  set  of  all  one-step  motions  guaranteed  to 
reach  the  goal  without  quantization.  Consider  algorithm  One-Step  which  computes 
the  set  T  of  all  motions  guaranteed  to  reach  G  from  a  staurt  region  R: 

Algorithm  One-Step 


1 .  < —  R  'X.  J . 

2.  T  J  -7rj(^Ri  -B{G)y 

3.  Return  any  0  £  T. 

In  a  sense,  Semi-Plan  and  One-Step  employ  the  non-directional  backprojection 
to  effect  “quantifier  elimination.”  That  is,  the  decision  problem  for  guaranteed 
one-step  strategies  is 

30  RcBe{G).  (3) 

One-Step  provides  a  way  to  eliminate  the  quantifier  and  in  fact  to  generate  all  0 
satisfying  (3).  For  two  step  strategies,  we  have  the  decision  problem 

3^2,  3^1  (4) 


' 


”  ’  A.*  •  **  -  ”  •  '  •  '  - ^  1“  ^  V  . 


Semi- Plan  is  an  algorithm  for  eliminating  the  outer  quantifier,  and  in  fax:t,  given 
an  ^1,  to  generate  all  62  satisfying  (4).  Taking  this  view,  we  can  characterize  One- 
Step  as  an  exact  algorithm  for  planning  guaranteed  strategies  in  a  planar  polygonal 
environment.  By  “exact,”  we  mean  that  it  does  not  rely  on  quantizations  or  approx¬ 
imations,  and  that  precise  bounds  are  known.  Similarly,  we  can  view  Semi-Plan  as 
a  “semi-excict”  algorithm  for  two-step  strategy  generation. 

This  is  just  a  start,  however,  much  work  remains: 

1.  The  combinatorial  bounds  on  B{G)  can  probably  be  improved.  It  remains  to 
prove  or  disprove  the  following  conjecture: 

Conjecture:  Given  a  goal  G  of  constant  size  and  an  arrangement  of  input  polygons 
V  of  size  0(n),  B{G)  can  be  computed  in  time  O(n^logn)  and  space  O(n^). 

2.  An  exact  version  of  Semi-Plan  could  lead  to  an  exact  algorithm  for  planning 
multi-step  giiaranteed  strategies.  Semi-Plan  is  merely  semi-exact.  We  would  like 
to  eliminate  the  “inner  quantifier”  in  (4),  and  thus  avoid  the  task  of  quantizing 

-space.  This  would  yield  an  exact  algorithm  for  planning  two-step  guaranteed 
strategies.  One  approach  would  be  to  introduce  a  new  axis  to  generalized  configu¬ 
ration  space,  Ji,  which  represented  the  space  of  all  possible  first  motions,  {  }.  We 

then  might  Uft  B{G)  to  B{G)  x  Ji,  and  construct  its  backprojection  B{B{G)  x  Jj) 
in  the  space  C  x  J  x  Ji.  In  this  case,  however,  instead  of  a  discrete  set  of  critical 
02  values,  we  obtain  a  set  of  critical  curves  in  the  ^1-^2  plane.  The  critical  slice 
algorithm  will  be  correspondingly  more  complicated,  and  remains  to  be  general¬ 
ized  to  this  czise.  This  approach  would  also  require  incrementing  the  dimension 
of  generalized  configuration  space  (by  1)  at  each  backchaining  step.  This  increase 
in  dimensionality  is  consistent  with  known  lower  bounds  on  the  motion  planning 
problem  with  uncertainty  [Canny  and  Reif]. 

3.  The  non-directional  backprojection  is  our  key  tool  in  developing  an  exact  algo¬ 
rithm  for  computing  guaranteed  strategies.  Similarly,  we  would  like  to  obtain  exact 
algorithms  for  computing  EDR  strategies.  The  key  theoretical  tools  here  would 
be  the  non-directional  weak  backprojection  and  non-directional  forward-projection. 
The  same  combinatorial  bounds  hold  for  these  non-directional  objects.  It  remains 
to  develop  exact  algorithms  for  their  set  difference,  for  determining  non-directional 
sticking,  zmd  for  distinguishability.  One  approach  to  an  exact  algorithm  for  multi- 
step  EDR  planning  might  be  as  follows.  Above,  we  suggested  how  an  exact  algo¬ 
rithm  for  multi-step  guaranteed  strategy  generation  might  be  devised.  [Brost]  has 
suggested  a  backchaining  EDR  planning  algorithm  which  can  generate  multi-step 
plans,  each  step  of  which  is  a  strong  EDR  strategy.  (See  chap.  III).  By  using 


the  non-directional  weak  backprojection  (in  place  of  the  directional  weak  backpro- 
jection)  in  such  a  backchaining  planner,  an  exact  algorithm  for  multi-step  EDR 
planning  might  be  constructed. 

4.  The  exact  algorithms  should  be  extended  to  more  general  configuration  spaces. 
Model  error  should  be  permitted.  As  above,  the  topology  of  the  non-directional 
backprojection  will  now  become  critical  along  hypersurfaces  in  the  resulting  gener¬ 
alized  configuration  space. 

Despite  the  apparent  difficulties  in  these  extensions,  I  feel  that  using  the  non- 
directional  backprojection  is  a  promising  approach  to  the  strategy  generation  prob¬ 
lem.  In  particular,  it  is  a  principled,  exact  algorithm  for  generating  compliant 
motion  strategies.  All  previous  theoretical  amd  implemented  fine-motion  plaimers 
[LMT,E, Mason, D, Buckley] — including  LIMITED — essentially  employed  or  suggested 
an  exhaustive  search  which  quantized  or  enumerated  the  set  of  possible  commanded 
motions.  In  order  for  fine- motion  planners  to  be  practical,  more  study  of  the  gen¬ 
eration  problem  is  required. 

14.5.  Complexity  and  Theoretical  Results 

Above,  we  described  a  polynomial  time  exact  algorithm  for  generating  one-step 
guaranteed  compliant  motion  strategies  amidst  planar  polygonal  obstacles.  We  now 
briefly  address  the  general  case  of  generating  guaranteed  r-step  compliant  motion 
strategies.  Assinne  sticking  termination,  so  that  for  all  $  and  all  R, 

F,eiR)  =  push,(F,(i?)). 

By  aneilogy  with  the  non-directional  backprojection,  we  can  define  the  non- 
directioneil  forward  projection.  Now,  we  observe  that  all  directional  projection  sets 
Eire  semi-algebraic  (s.a.).  Then  by  the  lemma  on  critical  values  of  B{G),  so  axe  the 
non-directional  projection  sets.  Furthermore,  when  R  has  constant  size,  the  lemma 
shows  that  the  non-directionaJ  projection  sets  have  descriptions  (as  s.a.  sets)  that 
are  polynomial  in  the  size  of  the  input  arrangement  V. 

In  the  following  definition,  we  eissume  that  the  control  uncertainty  cone  Bgc  is 
encoded  by  an  angular  error  bound  iCc- 

Definition:  The  planar  compliant  motion  planning  problem  with  sticking  termi 
nation  is  defined  as  follows.  Given  a  polygonal  start  region  R  of  constant  size,  ar, 
integer  r,  a  polygonal  environment  V  of  size  n,  control  uncertainty  Cg,  coefficient 
of  friction  pt,  and  a  polygonal  goal  G  of  constant  size,  find  a  sequence  of  r  mo¬ 
tions  9i, . .  .Br  such  that  each  motion  terminates  in  sticking,  and  the  final  motion 
Br  terminates  in  the  goal.  Or,  if  no  such  r-step  strategy  exists,  then  say  so. 


Theorem:  The  "planar  compliant  motion  planning  problem  with  sticking  termina¬ 
tion  is  decidable  in  time 

Proof.  Let  po  1  •  •  •  5  Pm  G  3^^  •  We  define  the  predicates 

feiPi,P2)  P2Gi=«(pi)  (5) 

and 

fe(Pi,P^)  P2  e  F^0(pi).  (6) 

Clearly,  definition  (6)  is  equivalent  to 

/«(Pi,P2)  fe{p\,P2)  ^  stick e{p2)-  (7) 

We  have  shown  how  in  polynomial  time  to  compute  a  quantifier-free  polynomial¬ 
sized  formula  (in  n)  for  the  s.a.  set  F(pi) — the  non- directional  forward  projection 
of  Pi.  It  remains  to  show  that  (5),  and  consequently  (6)  are  polynomial-sized  pred¬ 
icates.  Now,  d  6  S^,  Pi  €  9?^,  and  p2  G  Consider  /${•,•)  as  a  predicate  on 
a  5D  space  x  9?^  x  3?^,  that  is,  as  /(^,Pi,P2)-  We  can  obtain  a  bound  on  the 
complexity  of  /  by  enumerating  all  possible  edges  of  Fs{pi )  as  d  and  pi  vary.  These 
edges  then  sweep  out  developable  algebraic  surfaces  in  the  domain  of  the  predicate. 
There  are  four  types  of  edges  that  can  bound  Ftf(pi ): 

a.  An  edge  a  of  a  generalized  configuration  space  obstacle.  These  edges  sweep 
out  n  surfaces  of  the  form  5^  x  x  e<. 

b.  A  free-space  edge  anchored  at  a  vertex  vj  of  a  generalized  configuration  space 
obstacle  and  parallel  to  the  left  or  right  edge  of  the  velocity  cone.  Let  r{vj,0) 
denote  the  infinite  ray  anchored  at  Vj  at  orientation  9.  Then  type  (b)  edges 
sweep  out  2n  surfaces  of  the  form  lj^({  ^  )  x  3?^  x  r{vj,9  ±  Cc))- 

c.  A  free-space  edge  anchored  at  pi  and  parallel  to  the  left  or  right  edge  of  the 
velocity  cone.  These  edges  sweep  out  2  surfaces  of  the  form 

X  {pi  }  X  r(pi,^  ±  Cc) 

d.  A  partial  edge  of  a  generalized  configuration  space  obstacle.  Let  v\ ,  V2  be  the 
vertices  of  a  generalized  configuration  space  obstacle  edge.  A  partial  generalized 
configuration  space  edge  can  start  at  v\  or  V2  and  extend  to  u',  where  v'  is  a 
vertex  of  a  type  (b)  or  (c)  free-space  edge.  Clearly  v'  simply  arises  as  the 
intersection  of  a  type  (a)  surface  with  a  type  (b)  or  (c)  surface. 

By  enumeration,  we  clearly  obtain  a  linear  (0[n))  bound  on  the  number  of 
surfaces  in  the  5D  domain  of  /.  The  arrangement  of  these  surfaces  has  polynomial 
size;  in  particular,  it  has  0(nf)  critical  values.  Hence  we  may  conclude  that  /  is  a 
predicate  of  polynomial  size  in  n. 


Now,  define 


^{Po  1  •  •  •  )  Pmt  )  •  •  •  )  )  ■' — ''  fsi  (POt  Pi}  ^  (Pl  >  P2)  A  •  •  •  A  (Pm  —  1 1  Pm  )■ 

(8) 

Since  (6)  has  polynomial  size  in  n,  clearly  the  predicate  (8)  has  polynomial  size 
in  n  as  well.  Furthermore,  it  is  quantifier-free. 

Now,  we  let  the  points  pj  serve  as  via  points  (sometimes  known  as  switch- 
points)  for  the  strategy.  We  quantify  over  all  possible  via  points  achievable  by  the 
motion  strategy  ^i, . . . , ^r-  By  letting  m  be  r,  this  is  sufficient. 

We  can  formulate  the  question  of  the  existence  of  an  r-step  strategy  as  a  deci¬ 
sion  problem  within  the  theory  of  rezd  closed  fields: 

(3^1,. ..,^r) 

^Vpo,...,Pr  ^(po  €  i?)  A  J^(pO,...,Pr,^l,---,^r)j  =>  (Pr  6  G)^. 

Now,  deciding  sentences  in  the  theory  of  real  closed  fields  is  known  be  doubly- 
exponential  only  in  the  number  of  quantifier  alternations.  More  specifically,  the 
truth  of  a  Tarski  sentence  for  k  polynomials  of  degree  <  d  in  r  variables,  where 
a  <  r  is  the  number  of  quantifier  alternations  in  the  prenex  form  of  the  formula, 
can  be  decided  in  time 


(see  [Grigoryev]).  We  have  a  =  2,  and  hence  (9)  can  be  decided  in  time  □ 

This  theoretical  result  is  of  interest  for  the  following  reasons.  First  of  all,  the 
general  compliant  motion  planning  problem  with  uncertainty  (in  3D)  is  known  to 
be  hard  for  non-deterministic  exponential  time  [Canny  and  Reifj.  This  means  that 
any  algorithm  for  the  problem  takes  at  least  doubly-exponential  time  in  the  worst 
case.  In  this  section,  we  have  introduced  restrictions  on  the  problem  which  make  it 
more  tractable.  These  restrictions  are: 

•  The  configuration  space  is  the  plzme,  where  directional  forward  projections 
have  linear  size.  (In  3D  they  can  have  exponential  size).  A  key  step  in  otu 
construction  was  then  to  show  that  the  non-directional  backprojection  B{G) 
hzis  polynomial  size. 

•  Sticking  termination  is  used. 

•  The  maximum  number  of  steps  in  the  strategy  is  given  as  input  to  the  algo¬ 
rithm. 


With  these  restnctions,  the  problem  becomes  decidable  in  time  exponential  in 
r.  In  fact,  we  conjecture  that  for  a  great  number  of  planning  problems,  r  is  in  fact  a 
small  constant.  When  r  may  be  so  regarded,  we  effectively  obtain  a  polynomial-time 
algorithm  for  this  restricted  planar  motion  planning  problem  with  uncertainty. 

It  might  have  been  possible  to  devise  these  restrictions  a  priori,  from  a  strictly 
complexity-theoretic  viewpoint.  However,  I  believe  that  only  after  reading  the  pre¬ 
vious  chapters  does  it  become  clear  that  these  restrictions  are  physically  meaningful, 
and  in  fact  define  a  useful  and  interesting  subclass  of  planning  problems.  In  a  way, 
this  thesis  has  been  an  exploration  of  problems  solvable  within  these  restrictions. 
From  this  perspective,  I  believe  it  is  reasonable  to  conjecture  that  a  large  class  of 
planning  problems  do  fall  under  this  rubric. 


Of  course,  this  is  only  a  start.  From  the  standpoint  of  developing  theoreti¬ 
cal,  “exact”  algorithms,  we  have  only  addressed  the  problem  of  planning  certain 
restricted  classes  of  guaranteed  strategies  in  the  plane.  It  remains  to  consider  exact 
algorithms  in  higher-dimensional  configuration  spaces,  model  error,  EDR,  and  more 
sophisticated  termination  conditions. 


15.  Conclusions 


15.1.  Summary 

This  thesis  offers  two  main  contributions  to  the  theory  of  manipulation.  The 
first  is  a  technique  for  planning  compliant  motion  strategies  in  the  presence  of 
model  error.  The  second  is  a  precise,  geometrical  charaterization  of  error  detection 
and  recovery  (EDR).  These  led  to  a  constructive  definition  of  EDR  plzms  in  the 
presence  of  sensing,  control,  and  model  error.  These  more  general  strategies  are 
applicable  in  assembly  planning  where  guaranteed  plans  do  not  exist,  or  are  diflBcult 
to  find.  We  tested  the  EDR  theory  by  implementing  a  planner,  LIMITED,  and 
running  experiments  to  have  LIMITED  automatically  synthesize  EDR  strategies. 

A  number  of  mathematical  tools  were  developed  for  the  EDR  theory.  First,  we 
considered  compliant  motion  planning  problems  with  n  degrees  of  motion  freedom, 
and  k  dimensions  of  variational  geometric  model  uncertainty.  We  reduced  this 
plaiming  problem  to  the  problem  of  computing  preimages  in  an  (n  +  A:)-dimensional 
generalized  configuration  space,  which  encompasses  both  the  motion  amd  the  model 
degrees  of  freedom,  and  encodes  the  control  uncertainty  as  a  kind  of  non-holonomic 
constraint.  We  also  showed  how  pushing  motions  could  be  planned  using  generalized 
configuration  space.  In  addition  to  the  assembly  domain,  generalized  configvuation 
space  was  shown  to  serve  as  a  “map”  for  planning  sensing  and  motion  strategies  for 
a  mobile  robot  amidst  partially  known  obstacles. 

Next,  we  characterized  EDR  strategies  geometrically  via  the  EDR  region  H. 
Determining  whether  a  strategy  satisfied  the  EDR  axioms  was  reduced  to  a  deci¬ 
sion  problem  about  forward  projections  and  preimages  in  generalized  configuration 
sp>ace.  Making  this  process  formal  and  algorithmic  required  a  detailed  investiga¬ 
tion  of  the  geometric  and  preimage  structure  of  the  EDR  regions.  The  Weeik  EDR 
theory  introduced  new  mathematical  tools  for  studying  multi-step  strategies — 
reachability  and  reCognizability  diagrams,  strong  and  weak  strategy  equivalence, 
linking  conditions,  and  strategy  composition.  A  variety  of  techniques  for  plaiming 
multi-step  EDR  strategies  were  investigated  and  unveiled  as  special  cases  of  the 
Weak  EDR  theory. 

Finally,  we  explored  the  complexity  of  EDR  planning.  We  derived  bounds  both 
for  the  implemented  planner  LIMITED,  and  for  theoretical  extensions.  While  in 
general  it  is  known  that  compliant  motion  planning  with  uncertainty  is  intractable, 
we  were  able  to  demonstrate  a  number  of  special  cjises  where  there  exist  efficient 
theoretical  algorithms.  In  particular,  we  showed  a  case  where  n  =  2,  fc  =  1  and 
containment  in  the  backprojection  could  be  computed  in  polynomial  time  ( lu.re  for 
n  =  3,  Jfc  =  0,  this  is  false  [CR]).  We  also  investigated  the  structure  of  tli<  uon- 
directional  backprojection  in  the  plane.  It  led  to  a  polynomial-time  algoriilnn  for 


computing  one-step  (guaranteed)  strategies,  and  a  singly-exponential  algorithm  for 
multi-step  strategies. 

15.2.  Future  Work 

A  niimber  of  research  directions  deserve  further  attention: 

15.2.1  Probabilistic  Strategies 

The  EDR  framework  should  be  extended  to  include  probabiHstic  strategies. 
At  the  moment  the  EDR  theory  essentially  provides  a  binary  test  for  recognizing 
an  EDR  strategy.  It  would  be  useful  to  have  a  method  for  deciding  which  of  two 
strategies  was  “better.”  We  sketched  a  way  of  formalizing  this  generalization  in  sec. 
7. 

15.2.2  Goals  and  EDR  Regions  in  Phase  Space-Time 

When  the  goal  is  specified  in  phase  space- time  as  the  product  of  a  cylinder  over 
a  generalized  configuration  space  goal  with  a  compact  time  interval,  our  geometrical 
characterization  of  EDR  satisfies  the  EDR  axioms.  Without  time,  or  with  goals  of 
the  form  v~^{G)  x  [t,  oo),  the  definition  of  H  does  not  completely  fulfill  the  EDR 
axioms.  This  is  because  it  is  possible  for  motions  sticking  in  H,  to  eventually  slide 
into  the  goal,  violating  the  principle  that  no  motion  shotild  be  terminated  as  a 
failure  when  serendipitous  goal  achievement  is  still  possible. 

This  area  deserves  further  research.  Future  directions  include:  Relaxations  of 
the  EDR  axioms,  probabilistic  control  strategies,  implementation  of  termination- 
predicates  with  time,  computation  of  time-indexed  forward  projections,  and  study 
of  the  structure  of  phase  space-time  goals. 

15.2.3  Algorithmic  Improvements:  Search  and  Efficiency 

Limited  currently  employs  a  great  deal  of  exhaustive  search.  The  space  of 
model  error  and  the  space  of  commanded  directions  are  exhaustively  quantized.  We 
have  demonstrated  certain  theoretical  results  using  critical  slices  of  the  projection 
regions  (sec.  6)  and  the  non-directional  backprojection  (sec.  14)  to  show  how 
exhaustive  search  may  be  avoided  by  examining  only  “relevant”  constraints.  This 
direction  should  be  explored  more  extensively. 

On  a  related  note,  LIMITED  is  slow.  We  have  demonstrated  efficient  theoretical 
algorithms  for  subproblems  in  the  EDR  theory.  These  algorithms  should  be  reduced 


to  practice.  EDR  planning  in  higher-dimensional  genereilized  configuration  spaces 
may  be  prohibitive  unless  faster  algorithms  are  found. 

15.2.4  How  Often  is  Planning  Hard? 

While  compliant  motion  planning  with  uncertainty  is  known  to  be  very  hard 
in  general,  this  does  not  mean  that  all  such  problem  are  hard.  We  desire  some  way 
of  talking  about  the  “space”  of  geometrical  planning  problems,  and  defining  a  kind 
of  measme  on  that  space.  Then  perhaps  one  could  determine  whether  the  problems 
which  eire  hard  for  exponential  time  are  of  “measure  zero”,  for  example. 

15.2.5  Provably  Good  Approximate  Algorithms 

Concomitant  with  our  conjecture  about  the  distribution  of  geometrical  prob¬ 
lems  is  the  observation  that  the  intractability  of  exact  solution  does  not  preclude 
the  existence  of  fast  approximate  algorithms.  It  would  be  very  useful  to  develop 
such  algorithms  and  show  that  they  are  provably  good  approximations. 

15.2.6  Different  Complexity  Measures 

In  developing  good  average-time  algorithms  for  EDR  plaiming,  it  would  be 
useful  to  measure  the  complexity  in  the  size  of  the  output.  For  example,  while  it 
is  true  that  the  forward  projection  in  3D  can  have  exponential  size,  it  seems  that 
there  are  many  problems  in  which  it  is  much  smaller.  Thus  it  would  seem  natural 
to  measure  the  complexity  of  planning  in  3D  by  the  complexity  of  the  forward 
projection. 

15.2.7  Hardness  of  EDR  vs.  Guaranteed  Planning 

Since  the  EDR  theory  contains  [LMT]  as  a  subtheory,  it  appears  a  priori  at  least 
as  hard  to  decide.  However,  consider  the  following  “intuitionist”  argument:  many 
“hard”  problems,  requiring  exponential-length  guaranteed  plans  that  take  doubly- 
exponential  time  to  generate,  may  admit  “short”  EDR  plans  that  can  be  generated 
easily.  For  ex2imple,  the  peg-in-hole  insertion  strategy  with  model  error,  or  the 
gear-meshing  plan,  may  require  very  long  plans  if  the  plans  must  be  guaranteed. 
However,  we  can  find  2-step  EDR  plans  for  these  problems.  This  intuition — wliich 
is  a  heuristic  claim,  so  the  reader  is  advised  to  proceed  with  caution — shmiVl  be 
verified  or  disproven. 


15.2.8  Weak  EDR  Theory 


The  weak  EDR  theory,  v’^ile  still  in  its  infancy,  has  already  yielded  some 
interesting  results  and  a  fairly  clean  mathematical  framework  for  studying  multi- 
step  strategies.  The  key  idea  behind  the  weak  EDR  theory  is:  given  a  collection 
of  goals  {  }  (possibly  including  H),  we  consider  all  unions  of  the  subcollections 

to  get  some  measure  of  weakest  recognizability.  This  is  perhaps  the  most  exciting 
theoretical  area  for  future  work. 

As  an  immediate  goal,  the  linking  conditions  should  be  extended  for  time- 
indexed  forward  projections. 

15.2.9  Dynamic  Model 

The  dynamic  model  in  the  EDR  theory  should  be  tested,  by  trying  out  the 
EDR  plans  using  actual  robots.  The  dynamic  model  should  also  be  extended,  to 
incorporate  second-order  dynamics,  impact,  and  deformation. 

15.2.10  Computing  Projection  Sets 

When  rotations  and  compliant  motion  are  allowed,  we  do  not  know  of  exact 
algorithnas,  even  in  principle,  for  computing  projection  sets.  For  example,  the 
computation  of  forward  projections  is  not  immediately  decidable  within  the  theory 
of  real  closed  fields.  This  is  because  the  physics  of  motion  are  essentially  specified 
“differentially,”  that  is,  by  a  mapping  that  sends  a  configuration  x  E  Q  and  a 
commanded  motion  0  €  5"  (where  n  -f  1  is  the  dimension  of  C),  to  a  cone  Bc{x,9) 
in  the  tangent  space:  ^ 

^  X  5”  —*  cones  in  TQ 

{x,9)  I-+  Bc{x,9). 

Thus  we  have  a  differential  specification  of  the  possible  motions  Bc{x,  9)  at  each 
point  X.  The  cones  at  each  point  specify  a  parametric  family  of  vector  fields — a  field 
of  cones  to  be  precise.  The  integral  curves  for  this  family,  however,  may  not  be  al¬ 
gebraic  in  general.  Good  approximate  algorithms  are  needed  to  construct  bounding 
algebraic  envelopes  about  the  image  of  this  family  of  curves.  For  example,  assuming 
that  an  integral  curve  has  a  power  series,  it  is  possible  to  construct  a  recurrence 
relation  for  the  coefficients  of  the  series.  They  can  be  generated  deterministically 
to  the  accuracy  desired.  Randy  Brost^  has  investigated  other  numerical  techniques 
for  constructing  integral  curves  corresponsing  to  trajectories  in  the  forward  pro¬ 
jection.  This  is  an  interesting  area  for  future  research.  In  particular,  it  could  be 

*The  space  of  “cones  in  TQ"  can  be  formalized  as  an  appropriate  tensor  bundle  over  Q. 

^[Personal  Communication]. 


applied  to  the  “full”  4-dimensional  gear  meshing  problem  where  a  commanded  pme 
rotation  of  the  gripped  gear  could  induce  translations  or  rotations  of  either  gear. 
Such  algorithms  might  adso  be  applied  to  compute  projection  sets  under  different 
dynamics. 

15.2.11  Higher-Level  Primitives 

The  “primitives”  in  the  EDR  and  [LMT,E]  theories  are  somewhat  low-level — 
they  consist  of  commanded  generalized  damper  motions.  While  it  is  easy  to  describe 
such  motions,  their  effects  can  be  complex.  It  would  be  useful  to  develop  a  theory 
of  planning  with  higher-level  primitives  that  was  still  geometrical  in  character. 

15.2.12  Planning  Paradigms 

Different  planning  paradigms  for  EDR  planning  should  be  explored.  LIMITED 
is  a  forward-chaining  planner.  [Brost]  is  developing  a  backchaining  EDR  planner. 
It  would  be  interesting  to  integrate  and  compare  these  techniques. 

15.2.13  Functional  vs.  Geometrical  Descriptions  of  Goals 

Some  of  the  greatest  and  most  interesting  unsolved  problems  in  geometrical 
robotics  lie  in  the  interaction  of  functional  and  geometrical  descriptions  of  goals.  In 
particular,  we  would  like  to  devise  algorithms  for  computing  a  geometrical  goal  re¬ 
gion  given  a  fimctional  description — for  example,  a  quantified  difference  equation — 
for  the  desired  behavior  in  the  goal  state.  Conversely,  we  would  hke  to  be  able  to 
infer  a  functional  description  of  the  goal  from  its  geometriced  aspects.  The  latter 
would  be  useful  in  automatically  generating  termination  predicates  to  recognize  the 
goal. 

I  believe  that  EDR  is  an  exciting  and  fruitful  area  for  future  research.  Many 
of  the  directions  above  could  be  taken  as  criticisms  of  the  theory — ^for  example, 
that  it  is  too  slow,  or  may  require  exhaustive  search.  However,  I  would  rather  view 
these  as  criticisms  not  of  the  EDR  theory,  but  rather  of  the  state  of  the  art  in  EDR 
implementation,  that  is  LIMITED.  In  particular,  if  five  years  ago  one  had  surveyed 
researchers  in  robotics  and  eisked  them  what  to  do  about  model  error  and  EDR, 
I  beheve  that  the  general  response  would  have  been  “I  don’t  know;  it’s  a  good 
problem.”  Now  at  least  we  have  a  systematic  theory  of  model  error  and  EDR,  and 
are  faced  with  the  test  of  reducing  it  to  practice. 


APPENDICES 


§A.l.  A  Note  On  Geometry 

Our  definitions  of  phase  space,  and  phase-space  goals  have  been  primarily  set- 
theoretic.  These  sets  have  considerable  additional  structure,  which  is  a  good  thing, 
for  otherwise  there  would  be  no  hope  of  computing  them.  The  geometry  of  these 
regions  is  accessible  by  viewing  phase  space  as  the  tangent  bimdle  to  (gene’-alized) 
configuration  space.  This  gives  it  structure  both  as  a  differentiable  manifold  and 
a  vector  bimdle;  ir  is  the  canonical  covering  map.  The  moving  object’s  moment  of 
inertia  tensor  defines  a  field  of  inner  products  on  the  tangent  bundle,  providing  a 
natural  choice  for  a  Riemannian  metric.  For  example,  to  see  that  Z{Hg)  is  “well- 
behaved”,  observe  that  it  is  the  image  of  a  zero-section  (see  below)  of  the  tangent 
bimdle,  and  so  it  is  an  embedding  of  H,.  This  geometric  point  of  view  is  crucial 
to  a  computational  analysis;  it  is  developed  in  more  detail  here,  and  earlier  in 
[Erdmann]. 

Notation: 

Some  readers  may  still  wish  to  continue  thinking  of  the  tangent  bundle  to 
configuration  space  as  “Position-space  x  Velocity-space.”  This  is  set-theoretically 
correct,  although  it  ignores  its  topological,  algebraic,  differential,  and  geometric 
structure.  Set  theoretically,  the  tangent  space  Tg  at  a  configuration  x  may  be 
thought  of  ^ls  the  collection  of  all  velocities  (or  forces)  “at”  a  configuration  x.  That 
is,  the  tangent  space  at  i  is  the  cylinder  7r“^(x)  endowed  with  a  vector  space 
structure.  It  has  the  same  dimension  as  the  (generalized)  configuration  space.  The 
tangent  bundle  is  set-theoretically  the  disjoint  union  of  all  tangent  spaces.  It  has 
twice  the  dimension  of  the  configuration  space.  If  C  is  a  configuration  space  and 
TC  its  tangent  bundle,  a  section  of  the  tangent  bundle  is  a  map  s  :  C  — +  TC  such 
that  TT  os  is  the  identity  on  C.  Of  particular  interest  is  the  zero  section,  which  sends 
a  configuration  to  the  distinguished  zero-velocity  in  its  cylinder. 

§A.2.  A  Formal  Review  of  Pre-Images 

A  motion  strategy  is  a  commanded  velocity  together  with  a  termination  predi¬ 
cate  which  monitors  the  sensors  and  decides  when  the  motion  has  achieved  the  goal. 
The  actual  path  followed  depends  on  the  control  uncertainty,  but  we  require  that  it 
satisfy  generalized  damper  dynamics  (see  (4),  below).  Given  a  me^lsu^ed  position 
pj  in  configuration  space,  a  set  R,  and  a  collection  of  goals  {G^},  [LMT]  define 
5(p3,  R,  {  Gff  })  to  be  the  set  of  all  commanded  velocities  Vg  such  that  the  termina- 
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tion  predicate,  knowing  the  initial  measured  position  pj  corresponds  to  an  actual 
position  p  in  iZ,  is  guaranteed  to  signal  success.  We  denote  the  position  sensing 
error  ball  about  p  by  Bep{p).  A  pre-image  of  a  collection  of  goals  {  }  relative  to 

a  set  iZ  is  the  set 

})  =  {  P  €  iZ  I  VpS  €  5ep(p),  5(po*,  iZ,  {  })  7^  0  }, 

that  is,  the  set  of  all  positions  p,  such  that,  for  all  measured  positions  pj  consistent 
with  p,  there  is  some  commanded  control  velocity  Vq  such  that  the  termination 
predicate  is  guaranteed  to  signal  success. 

Analogoiisly,  define  the  directional  pre-image  with  respect  to  a  nominal  com¬ 
manded  velocity  by 

PrM  })  =  {  p  6  I  Vp;  e  B„(p),t,;  €  S(p;,  h,  { }) }. 

The  directional  pre-image  is  the  set  of  points  which  are  guaranteed  to  recogniz¬ 
ably  enter  the  goal  under  a  particular  commanded  velocity  uj.  The  weak  preimage 
is  the  set  of  points  which  could  possibly  enter  the  goal  recognizably,  given  fortuitous 
sensing  and  control  events. 

The  backprojection  of  a  goal  (with  respect  to  a  commanded  velocity  uj)  consists 
of  those  points  guaranteed  to  enter  the  goal.  Recognizability  of  the  entry  plays  no 
role. 

The  forward  projection  of  a  region  iZ  (with  respect  to  Vg)  is  the  set  of  positions 
and  velocities  (considered  as  ordered  pairs)  which  are  possibly  reachable  from  iZ 
under  Vg. 

For  a  comprehensive  account,  see  [LMT,E].  For  convenience,  we  summarize 
the  notation  here.  While  historically  the  subscript  9  has  been  used  to  indicate  the 
“angular  direction”  of  a  commanded  motion,  we  will  employ  it  as  an  arbitrary  index 
for  motion  strategies,  commanded  velocities,  and  termination  predicates. 

Symbol  Table; 

Vq  nominal  commanded  velocity. 

Vo  eictual  commanded  velocity. 

V  actual  velocity. 

V*  sensed  velocity. 

p  actual  position. 

p*  sensed  position. 

Becivo)  control  imcertainty. 

Be„(v)  velocity  sensing  uncertainty, 

Btp{p)  position  sensing  uncertainty. 

Fs{R)  forward  projection  of  iZ  for  Vq  =  vg. 
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^V-V- v.-w'.-.- 


■  k  f  '/vv.-.v  r.  .V,  V  -A*- 


_ 


Fe{R)  natural  projection  nF  into  C-space  . 

Bff(G)  directional  backprojection  of  G  under  Vq  =  vg. 

Fr({  G^  })  non-directional  pre-image. 

■PR,fl({  G^})  directional  pre- image. 

Pr{{  G^  })  non-directional  weak  pre-image. 

G^  })  directional  weak  pre-image. 

For  notational  felicity,  we  will  define  Be{{  G,H  })  =  B${G  U  H). 

§A.3.  On  the  Geometry  and  Physics  of  Generalized  Configuration 
Space 

We  now  disctiss  the  geometry  and  physics  of  generalized  configuration  space 
somewhat  more  formally.  Generalized  Configuration  Spzu:e  is  a  smooth  manifold. 
Intrinsically  it  is  not  different  from  the  configuration  spaces  considered  in,  for  ex¬ 
ample,  [LMT,  E,  D,  C].  We  must  define  a  system  of  dynamics  for  generalized  config¬ 
uration  space  in  order  to  define  motions.  Furthermore,  we  must  define  how  sensing 
generalizes. 

Let  C  be  a  smooth  configuration  space.  Let  J  be  an  airbitrary  set  which  will 
index  the  possible  configuration  space  environments.  Generalized  Configiiration 
Space  is  ^  =  C  X  J,  and  a  particular  “world”  is  simply  C  x  {  a  }  for  a  €  J.  Thus 
we  let  {Ca}aeJ  a  set  of  configuration  spaces,  each  containing  configuration 
space  obstacles.  The  ambient  space  for  each  €<,  is  some  canonical  C,  which  is  the 
configuration  space  for  the  degrees  of  freedom  of  the  moving  object.  Q  is  simply 
the  naturzd  product  representing  the  ambient  space  of  the  disjoint  imion  of  the  Ca- 
There  is  no  constraint  that  J  be  finite  or  even  coimtable.  Now,  assume  that  J  is  also 
a  smooth  manifold  (with  boundary),  although  as  we  shall  see,  this  is  not  a  serious 
restriction.  We  wish  to  define  a  “physics”  on  Q,  that  is,  a  set  of  laws  that  motions 
in  generalized  configuration  space  must  obey.  This  physics  will  be  expressed  as  a 
set  of  constraints  on  imcertainties  and  trajectories  in  the  tangent  bundle  TQ.  Write 
X  =  (x,a)  E  G-  A  particular  tangent  space  will  be  written  Tr=  Tx,a  =  Tx,aG-  We 
will  use  the  convention  that  the  analogs  of  mathematical  objects  in  the  pre-image 
framework  will  be  written  with  a  bar  in  the  model  error  framework.  Thus  a  velocity 
in  Tz^a  will  be  written  v  =  (u,  da)  where  v  €  TjC. 

(1)  There  is  infinite  position  sensing  uncertainty  in  J.  This  means  that  we  will 
define  the  generalized  position  uncertainty  as  Bep{x)  =  Bep{x)  x  J. 

(2)  Motions  are  possible  in  Q,  but  any  motion  must  stay  within  one  slice  of 
G,  say  C  X  {  ao  }.  (We  call  this  an  Qo-slice  of  G)-  Qq  is  the  actual  position  in 
J.  If  0  €  ToqJ  denotes  the  zero  velocity  in  the  tangent  space  to  J  at  oq.  then 
we  can  define  the  generalized  velocity  sensing  uncertainty  Be„(v)  =  Beviv)  x  (  0  }. 
Analogously,  the  generalized  control  uncertainty  is  Bec(i^)  =  Bec(vo)  x  {  0  ) .  These 


definitions  ensure  that  all  sensed  and  commanded  velocities  are  tangent  to  C  and 
have  zero  component  along  J. 

(3)  Define  a  generalized  trajectory  $  :  [0,  cx5)  TQ  by  $(t)  =  ($p(t),  $„(/)) 
=  ($p(t),a(t),  $„(t),da(t)),  where  (a:(t),dc.(<))  6  TJ. 

(4)  Let  /  G  TQ  be  a  force  tangent  to  generalized  configuration  space.  Then 
/  must  satisfy  the  damper  eqviation  /  =  B(v  —  uq).  In  truth,  this  is  a  generalized 
generalized  damper  constraint.  B  must  vary  smoothly,  that  is,  B  is  a  smooth  tensor 
field  on  In  practice,  we  constrain  B  to  be  diagonal  on  each  tangent  space  so  that 
it  cannot  cross-couple  TC  and  TJ. 

(5)  Let  p  =  and  u  =  $„.  p  and  v  must  be  related  via  the  integrability 
constraint 

P(0  =  P(0)+  /  Ht)dt. 

Jo 

(6)  vq  G  That  is,  the  generalized  actual  commanded  velocity  must  be 

consistent  with  the  error  boimds  on  the  generalized  nominal  commanded  velocity. 

If  $  obeys  these  constraints  (1-6)  we  say  it  is  a  trajectory  satisfying  the  damper 
equation  with  imcertainty  relative  to  a  comm2inded  velocity  Vq.  Such  a  trajectory 
is  constrained  to  have  a{t)  =  oq  and  da{t)  =  0  for  all  t.  The  latter  implies  that  the 
image  of  any  $  lies  in  the  submanifold  TC  x(Jx{0})  =  TC  x  J .  This  is  why 
we  can  think  of  TQ,  the  phase  sp2w:e  of  Q,  as  being  TC  x  J  instead  of  TC  x  TJ.* 
It  also  suggests  that  we  may  relax  the  constraint  that  J  be  smooth. 

[Erdmann]  generalizes  Euclidean  friction  cones  to  arbitrary  smooth  C.  The 
generalized  friction  cones  embed  in  TC.  Thus  friction  cones  embed  naturally  in 
generalized  configuration  spewje.  Of  course,  they  have  no  non-zero  component  in 
TJ. 

In  this  thesis,  when  there  is  no  possibility  of  confusion  we  have  dropped  the 
bar  notation  and  assume  that  in  generalized  configuration  space  all  quantities,  un¬ 
certainty  balls,  trajectories,  etc.,  are  barred,  i.e.,  generalized.  We  have  referred  to 
generalized  configuration  space  as  Q. 

The  definitions  and  results  for  pre-images  and  backprojections  [LMT,E]  in  C- 
space  generalize  mutatis  mutandis  to  Q  endowed  with  this  physics.  Thus  this  frame¬ 
work  reduces  the  guaranteed  motion  strategy  planning  problem  with  model  error 
to  computing  pre-images  in  a  somewhat  more  complicated,  and  higher-dimensional 
configuration  space. 

§A.4.  Derivation  of  the  Non-Holonomic  Constraints  for  Pushing 

The  previous  discussion  assumed  that  motion  across  J  was  impossible.  That  is, 
^This  is  only  acceptable  when  no  motion  across  J  is  possible!  See  §4.4. 


all  motion  is  confined  to  one  o-slice  of  generalized  configmation  space.  In  example 
(1),  this  is  equivalent  to  the  axiom  that  B  does  not  move  or  deform  under  an  applied 
force.  Such  an  axiom  makes  sense  for  applications  where  B  is  indeed  immovable, 
for  example,  if  A  and  B  are  machined  tabs  of  a  connected  metal  p2irt.  However, 
suppose  that  B  is  a  block  that  can  slide  on  the  table.  See  fig.  11.17.  Then  an  applied 
force  on  the  surface  of  the  block  can  cause  the  block  to  slide.  This  corresponds  to 
motion  in  J.  In  general,  the  effect  of  an  applied  force  will  be  a  motion  which  slides 
or  sticks  on  the  surface  of  B,  and  which  causes  B  to  slide  or  stick  on  the  table.  This 
corresponds  to  a  coupled  motion  in  both  C  and  J.  When  the  motion  maintains 
contact,  it  is  tangent  to  a  surface  S  in  generalized  configuration  space. 

Our  goal  is  to  generalize  the  description  of  the  physics  of  Q  to  permit  a  rigorous 
account  of  such  motions.  This  model  can  then  be  employed  by  an  automated 
planner.  Suck  a  planner  could  construct  either  Guaranteed  or  EDR  strategies  whose 
primitives  were  gross  motions,  compliant  motions,  and  pushing  motions. 

First,  we  must  determine  what  reaction  forces  are  in  generalized  configuration 
space.  By  this  we  mean  the  following.  Surfaces  in  configuration  space  share  many 
properties  with  real-space  surfaces.  When  pushed  on,  they  push  back.  They  have  a 
normal  in  configuration  space,  and  in  the  absence  of  friction  can  exert  forces  only  in 
that  normal  direction.  In  general  the  configuration  space  normal  is  different  from 
the  real-spaee  normal;  see  [E].  Furthermore,  [E]  developed  a  configuration  space 
analogue  of  the  classical  Coulomb  friction  cone  of  Cartesian  space.  It  defines  the 
range  of  reaction  forces  the  surface  can  exert,  and  may  be  employed  to  predict 
reaction  forces,  given  a  applied  force.  In  addition,  given  either  Newtonian  or  gener¬ 
alized  damper  dynamics,  the  configuration  space  friction  cone  can  be  used  to  predict 
whether  sticking  will  occur  on  a  surface,  given  a  cone  of  control  velocity  imcertainty. 
Under  damper  dynamics,  the  computation  at  a  point  is  especially  simple:  sticking 
can  occur  when  the  intersection  of  the  negative  velocity  cone  and  the  friction  cone 
is  non-trivial. 

We  wish  to  extend  these  methods  to  generalized  configuration  space.  Then, 
given  a  cone  of  applied  forces  as  in  fig.  11.17,  we  can  predict  the  cone  of  resxilting 
motions  in  Q.  That  is,  we  can  predict  the  motion  along  the  surface  of  B,  and 
the  motion  of  B  on  the  table.  In  particular,  given  a  control  uncertainty  cone,  we 
can  compute  whether  sticking  occius  on  the  surface  5  in  generalized  configmation 
space. 

In  this  analysis,  we  will  consider  a  force  exerted  by  the  robot  on  an  object 
B.  This  corresponds  to  an  application  of  force  on  a  surface  S  in  Q.  The  analysis 
will  apply  to  arbitrary  smooth  C,  J,  and  5,  for  an  arbitrary  rigid  object  B  whose 
configuration  space  is  J .  The  reader  may  imagine  B  and  S  as  usual  in  example  (1). 
In  example  (1),  however,  the  configuration  space  of  B  is  one-dimensional.  To  make 
the  example  more  interesting,  let  us  suppose  that  B  is  free  to  translate  and  rotate 
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on  a  planar  table.  So  C  is  as  before,  but  J  is  3?^  x  5^ . 

We  must  determine: 

•  What  is  the  normal  to  a  surface  S  in  generalized  configiiration  space? 

•  What  is  the  analogue  of  the  friction  cone  in  generalized  configuration  space? 

•  What  are  the  applied  forces  which  cause  sticking  at  a  point  on  5?  What  is 
their  geometric  interpretation  and  computation? 

•  How  may  reaction  forces  be  computed  geometrically,  using  the  generalized  fric¬ 
tion  cone  in  generalized  configuration  space? 

Our  analysis  is  applicable  imder  the  following  assiunption:  We  must  have  per¬ 
fect  knowledge  of  the  centers  of  friction  of  B  and  the  robot.  To  satisfy  this  as¬ 
sumption,  it  is  suflicient  to  assume  that  all  (read-space)  contacts  consist  of  a  finite 
number  of  points.  For  example,  we  might  model  the  contact  of  B  on  a  planar  table 
by  the  intersection  of  four  one-point  contacts.  Alternatively,  the  centers  of  friction 
are  known  when  the  pressure  distributions  are  known  precisely.  In  the  more  general 
case  where  the  pressure  distribution  is  not  known  precisely,  then  the  more  gen¬ 
eral  theory  of  pushing  described  by  [Mason,  82]  must  be  employed.  More  research 
is  needed  to  extend  this  more  restricted  theory  of  pushing  to  the  caise  where  the 
centers  of  friction  are  not  known  precisely.  One  possible  solution  is  to  introduce 
xmcertainty  in  the  center  of  friction  as  a  model  error  parameter  in  J. 

This  discussion  represents  work  in  progress.  In  particular,  the  method  de¬ 
scribed  for  the  computation  of  reaction  forces  is  meant  to  be  illustrative  of  the 
techniques  that  an  EDR  planner  would  require,  if  it  were  to  competently  plan 
pushing  motions.  I  hope  that  a  simpler  algorithm  can  be  found  before  a  practical 
planner  is  constructed. 

Normals  to  Surfaces  in  Generalized  Configuration  Space 

Let  C  and  J  be  the  configuration  spaces  of  the  robot  and  of  the  block  B,  respectively. 
Assume  that  the  reference  points  are  chosen  to  lie  at  the  centers  of  mass.  Typical 
configuration  spaces  we  will  consider  are  the  two  and  three  dimensional  groups  of 
Euclidean  motions,  3?^  x  and  3?^  x  S0(3).  The  definition  of  normals  in  the 
tangent  spaces  to  these  configxiration  spaces  depends  on  the  inner  product.  There 
is  a  natural  choice  for  the  Riemannian  metric;  see  [Arnold,  Abraham  and  Marsden, 

Ej. 

The  moment  of  inertia  tensor  of  the  robot  defines  a  field  of  inner  products 
on  C.  On  a  tangent  space  T^C,  write  the  inner  product  as  This  choice 

of  inner  product  is  “natural”  in  the  following  sense:  The  Riemannian  metric  i^  a 
quadratic  form  which  computes  the  correct  kinetic  energy  of  the  robot  moving  with 
generalized  velocity  v  at  configuration  x: 


X 


z 


A  Riemannian  metric  {•,•)<»  on  the  tangent  space  TqJ  is  defined  in  the  same 
way.  The  inner  products  may  be  combined  by  direct  sum  to  define  an  inner  product 
on  TxC  X  TaJ.  We  can  visualize  this  as  follows.  Since  the  moment  of  inertia 
operators  axe  symmetric  2-tensors,  we  can  view  them  as  square  matrices  and 
In  princip2d  axis  coordinates,  for  example,  both  will  be  diagonal  matrices  with 
unit  entries  for  the  Cartesian  dimensions,  and  the  squares  of  the  radii  of  gyration 
in  the  rotational  coordinates.  The  operation  of  on  two  velocity  vectors  u,  u  in 
TxC  is  defined  by 


{v,u)x  =  V^xU. 

Write  X  =  (x,  a)  as  usual.  Let  Wi,  W2  be  in  Tj^.  So  the  inner  product  on 
is  defined  by 


{wi,W2)x= 


(♦) 


Thus  the  direct  sum  of  the  inner  products  defines  a  field  of  inner  products  on 
Q.  Since  the  reference  points  are  at  the  centers  of  mass,  the  kinetic  energies  of  the 
robot  and  of  B  simply  add  without  cross-coupling.  Therefore  (♦)  defines  the  natural 
Riemannian  metric  on  generalized  configuration  space,  since  it  describes  the  kinetic 
energy  of  the  system. 

Now,  a  tangent  vector  {vc,Vj)  in  Tji?  corresponds  to  a  (generalized)  velocity 
of  Vc  of  the  robot  and  vj  of  the  object  B.  Generalized  damper  dyneimics  permits 
an  identification  of  forces  and  velocities.  Thus  the  pair  {fcifj)  with 


fc  €  TxC 

fj  e  T^J 

corresponds  to  a  generalized  force  of  fc  applied  at  the  center  of  mass  of  the  robot, 
and  a  generalized  force  of  fj  applied  at  the  center  of  mass  of  B. 

Note  that  the  inner  product  (*)  defines  the  normal  space  to  the  surface  S  in 
generalized  configuration  space.'*  See  fig.  II.  IS.  The  normal,  in  general,  can  be 
transverse  to  J.  Hence  S  can  exert  reaction  forces  across  J  even  when  the  applied 
force  lies  exclusively  in  TxC.  In  the  figure,  this  implies  that  pushing  on  the  side  of  B 
results  in  a  reaction  force  across  J,  causing  B  to  slide.  In  generalized  configuration 
space  this  is  simply  viewed  as  applying  a  force  to  the  surface  5,  which  exerts  a 
reaction  force  across  J.  Since  the  resultant  force  is  across  J,  the  motion  in  ^  will 
be  in  that  direction,  tangent  to  5. 

‘*This  is  independent  of  the  choice  of  generalized  damper  or  Newtonian  dynamirs. 


Construction  of  the  Sticking  Cone 


We  now  derive  a  geometrical  object  called  the  sticking  cone.  It  represents  the  cone 
of  forces  (/c,  fj)  that  can  cause  sticking  on  a  surface  in  Q.  Under  generalized  damper 
dyncimics,  the  sticking  cone  represents  the  commanded  velocities  that  can  result  in 
sticking. 

Henceforth,  all  forces  are  generalized  forces  unless  otherwise  indicated.  Keeping 
the  notation  above,  suppose  the  generzdized  force  (/c,  fj)  is  applied  at  configuraticn 
X  —  (i,  a),  which  lies  on  surface  S  in  generalized  configuration  space.  Now,  it  is 
clear  that  fc  acts  on  J?  as  well  as  on  the  robot.®  Assume  for  discussion  that  fc 
contains  no  torque  components.  Then  the  effect  of  fc  on  B  is  both  fc  acting  at 
the  center  of  msiss  of  B,  pirn  the  torque  induced  on  B  hy  fc.  Let  r*  be  the  radius 
vector  from  the  center  of  mass  of  B  to  the  point  of  application  of  fc  in  real  space. 
Then  the  effect  of  fc  on  B  can  be  written® 

fc  —  /c  +  X  /c  6  To,  J. 

The  torque  component  of  /*  must  also  be  normahzed  relative  to  the  inner  product 
on  TaJ]  this  is  not  indicated  above. 

Now,  of  course  it  is  also  true  that  fj  induces  an  force  on  the  robot.  For  example, 
if  in  example  (1)  the  block  B  is  pushed  at  its  center  of  mass,  then  a  force  can  be 
exerted  on  the  robot  when  the  robot  is  in  contact  with  B.  The  induced  force 
contributes  to  the  reaction  force  of  B  on  the  robot. 

Let  fr,c  denote  the  reaction  force  of  B  on  the  robot;  it  lies  in  TgC.  Let  frj 
denote  the  reaction  force  of  the  table  on  B\  it  lies  in  TaJ.  The  force  balance 
equations  for  static  equilibrium  are 


fc  +  fr,c  —  0 
/>  +  /*  +  fr,j  =  0- 

Now,  let  Kx  be  the  configuration  space  friction  cone  [E]  in  TjC,  and  Ka  be  the 
configuracion  space  friction  cone  in  TaJ.^  The  conditions  for  static  equilibrium  are 
then  expressible  as 


-fc  €  Kx 
-fj-ffeKa. 


{**) 


®Of  course,  this  is  only  true  when  the  robot  and  B  are  in  contact,  i.e,  x  €  5. 

®Under  quasi-static  assumptions. 

'  W'e  do  not  assume  that  the  center  of  friction  is  at  the  center  of  mass.  If  this  is  not 
the  case,  however,  the  friction  cone  must  be  placed  at  the  center  of  friction,  .  nd  then 
resloved  relative  to  the  center  of  mass  by  adding  the  resultant  torques. 


Thus  it  is  clear  that  the  range  of  reaction  forces  that  the  surface  S  can  exert 
is  exactly  the  direct  sum  of  the  two  friction  cones, 


A%  ©  Ka 

which  is  a  cone  in 

We  are  now  prepared  to  construct  the  sticking  cone.  Informally,  the  idea  in¬ 
volves  twisting  and  tilting  the  friction  cone  in  J  as  a  fimction  of  fc-  The  amount 
of  the  twist  and  tilt  is  determined  by  the  force  /*.  The  tilted  friction  cone  takes 
into  accotmt  the  “internal”  force  /*,  and  admits  a  geometrical  cadculation  of  sticking 
that  considers  only  the  applied  force  (/c/j)- 

We  can  rewrite  the  sticking  conditions  imder  static  equilibrium  (**)  as: 

^  (t) 

-//€  A'. +  {/•}, 

where  the  translation  operation  -|-  is  defined  byV’  +  {i£;}  =  {t;-(-t/;|t;€V’}. 

(t)  defines  a  cone  of  forces  in  the  tangent  space  to  generalized  configuration 
space  at  x.  We  may  write  it  sis® 

U  +  {/;))• 

-/ce/c. 

For  example,  in  the  case  with  no  torque,  f*  =  fg. 

The  cone  of  all  forces  (/ci/j)  satisfying  (f)  has  the  following  geometrical  in¬ 
terpretation.  The  force  /*  causes  a  translation  of  the  friction  cone  in  TaJ.  It 
parameterizes  a  family  of  cones  in  the  tangent  space  to  J.  The  imion  of  this  family 
defines  a  range  of  applied  forces  (/c,  fj)  that  can  cause  sticking  at  x.  For  example, 
consider  fig.  11.19.  Here  we  take  the  configuration  spaces  of  the  robot  and  of  B  to 
be  cartesian  planes  (3?^).  The  friction  cone  in  generalized  configuration  space  will 
then  be  four-dimension2d.  This  is  hard  to  draw;  we  have  selected  a  fixed,  negative 
normal  component  for  fj.  The  3D  force  space  at  x  represents  the  product  of  the  2D 
forces  that  can  be  exerted  by  the  robot  on  the  surface  of  B,  with  the  ID  tangential 
forces  that  can  be  applied  at  the  center  of  mass  of  J3.  An  applied  force  (/c,/>)  in 
the  cone  in  fig.  11.19  represents  a  combination  of  forces  that  causes  no  motion  in 
Q,  that  is,  neither  sliding  on  the  surface  of  B,  nor  of  B  on  the  table.  Note  that  the 
cone  in  G  is  skewed  out  of  the  embedded  tangent  space  to  C  at  i.  This  is  because 
when  a  force  is  applied  in  the  friction  cone  Kz,  the  block  B  can  slide  unless  an 
opposing  force  is  exerted  tangentially  at  the  center  of  m^lss  of  B. 

There  are  several  points  of  interest.  Note  that  the  new  cone  defined  by  (f), 
which  we  will  denote  AC,  changes  from  point  to  point  when  torques  are  permitted. 
This  is  because  the  radius  vector  changes  from  point  to  point.  Therefore  the  torque 

®0  denotes  the  direct  sum. 


components  of  the  generalized  force  /*  will  translate  the  friction  cone  in  TJ  by 
different  amounts  as  the  vector  r,  to  the  center  of  mass  of  B  varies.  This  effect  is 
similar  to  the  way  that  the  Erdmannian  friction  cone  changes  as  the  contact  moves 
in  rotation  spaee  [E].  However,  in  generalized  configuration  space  it  changes  even 
under  pure  translation. 

We  can  now  specify  a  geometrical  computation  to  determine  when  sticking 
occurs  at  i,  eissuming  generalized  damper  dynamics:  Simply  intersect  the  negative 
velocity  control  uncertainty  cone  with  AC.  If  the  intersection  is  trivial,  then 

sticking  camnot  occur.  If  the  intersection  is  non-trivial,  then  sticking  can  occur.  If 
the  negative  velocity  cone  lies  inside  AC,  then  sticking  must  occur.  See  fig.  11.19. 
Assume  it  is  impossible  to  apply  force  at  the  center  of  mass  of  B.  Therefore,  the 
velocity  cone  is  two  dimensional  and  lies  entirely  in  the  tangent  space  to  C  at  x;  it 
has  no  J  component.  This  two-dimensional  cone  is  intersected  with  the  3D  cone  AC 
to  determine  whether  sticking  is  possible  at  i. 

This  shows  that  the  computation  to  determine  whether  sticking  is  possible  at 
a  point  reduces  to  simple  geometric  cone  intersection. 

Computation  of  Reaction  Forces 


We  now  provide  a  geometrical  method  for  the  computation  of  reaction  forces  in 
Q.  That  is,  given  an  appUed  force  {fcfj)-,  we  show  how  to  compute  the  reaction 
forces  and  hence  the  resulting  motion.  Such  an  algorithm  is  required  by  a  plaimer 
since  it  is  necessary  to  predict  the  effect  of  a  commanded  force  on  the  motion  of  the 
robot  and  B.  We  will  assume  Newtonian  mechanics  in  this  section,  for  the  following 
reasons; 

•  It  is  not  clear  that  the  interaction  of  B  on  the  table®  surface  can  be  modeled 
accurately  by  a  damper. 

•  Under  generalized  damper  dynamics,  it  is  not  clear  whether  the  robot  velocity 
induced  by  a  force  /c  should  be  relative  to  the  object  B  on  which  it  shdes,  or 
relative  to  some  global  coordinate  frame. 

•  In  second-order  systems,  accelerating  reference  frames  can  introduce  fictitious 
forces.  Correspondingly,  under  generalized  damper  dynamics,  it  seems  that 
reference  frames  moving  at  constant  velocity  should  also  introduce  fictitious 
forces. 

It  rem^lins  to  give  a  principled  account  of  these  issues;  this  is  a  fruitful  area  for 
future  research. 

The  prediction  of  reaction  forces  imder  general  dynamic  conditions  is  compli¬ 
cated  by  the  fact  that  the  object  B  and  the  robot  may  accelerate  under  the  resultant 

®We  will  use  the  “table”  ais  the  name  for  the  surface  B  is  in  contact  with,  wiili  ihe 
understanding  that  the  surface  is  arbitrary. 
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force.  This  means  that  the  computation  of  reaction  forces  cannot  be  reduced  to  a 
simple  projection  onto  the  friction  cone.  The  re2ison  for  this  is  that  the  projection 
must  be  done  in  an  inertial  frame,  fixed  on  the  object  which  exerts  the  reaction 
force.  That  is,  the  apphed  force  must  be  expressed  in  a  frame  of  reference  which  is 
non-accelerating  relative  to  the  “bottom”  object  which  is  being  “slid  upon.” 

For  example,  consider  the  case  where  the  normal  aiccelerations  are  zero.  That  is, 
assume  that  contact  of  the  robot  on  B,  and  of  B  on  the  table  are  both  maintained. 
We  fix  a  reference  frame  L  on  B.  Then  the  reaction  force  of  B  on  the  robot 
may  be  foimd  by  projecting  the  effective  applied  force  in  L  onto  the  Erdmannian 
friction  cone  [Ej.  However,  the  effective  applied  force  is  not  known  a  priori:  the 
fictitious  force  due  to  the  acceleration  of  B  contributes  to  it;  this  acceleration  is 
also  an  unknown.  The  projection  relation  however,  still  holds.  In  general,  it  adds 
a  quadric  constraint  into  the  equations  of  motion.  The  simultaneous  solution  of 
these  equations  yields  the  reaction  force  and  resultant  acceleration.  (The  global 
coordinate  system  is  a  non-inertial  frame  for  the  table). 

To  summarize,  we  can  compute  the  reaction  forces  by  writing  down  the  equa¬ 
tions  of  motion  of  the  robot  and  B.  Then  we  add  the  constraints  from  the  projection 
relations  expressed  in  inertial  frames.  Their  simultaneous  satisfaction  yields  the  re¬ 
action  forces. 

Let  us  derive  the  reaction  forces  for  the  case  where  C  and  J  axe  both  3?^  x  5^ . 
The  analysis  follows  [E],  and  is  a  generalization  thereof.  We  will  asstime  one-point 
contacts  between  the  robot  and  B,  and  between  B  and  the  table.  Furthermore,  we 
assume  that  neither  contact  is  broken.  Centripital  accelerations  and  coriolis  forces 
are  not  considered.  This  is  a  reasonable  assumption  under  quasi-static  conditions, 
or  when  the  robot  and  B  are  only  rotating  slowly. 

We  will  use  the  following  notation: 

The  radii  of  gyration  of  the  robot  and  B,  respectively,  are  Pc  and  pj. 

The  masses  of  the  robot  and  B,  respectively,  are  rric  and  rrij. 

The  generalized  applied  force  in  C  is  fc  =  ifcifcifc)-  That  is,  the  applied 
force  is  {fc,fc)  ^-nd  the  applied  torque  is  Tc  =  Pcfc- 

The  generalized  applied  force  in  7  is  ^  That  is,  the  applied 

force  is  and  the  applied  torque  is  tj  =  Pjfj. 

The  configuration  space  normal  in  C  is  nc  =  (nj,  n\).  The  unit  real  space 
normal  is 

The  configuration  space  normal  in  J  is  nj  =  (n®,ny,np.  The  unit  real  space 
normal  is  n?  =  (nj,nj). 

The  magnitude  of  the  normal  reaction  force  at  the  contact  point  between  the 
robot  and  B  is  /„,c- 

The  magnitude  of  the  normal  reaction  force  at  the  contact  point  between  B 
and  the  table  is  fnj- 
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Fc  is  the  vector  from  the  point  of  contact  of  the  robot  on  B  to  the  reference 
point  of  the  robot. 

Fj  is  the  vector  from  the  point  of  contact  of  B  on  the  table  to  the  reference 
point  of  B. 

In  the  absence  of  friction,  the  equations  of  motion  are  therefore 


fn,cnl 

+ 

// 

= 

mcOc 

fn,cnl 

+ 

// 

= 

rrica” 

fn,cnl 

+ 

Tc 

= 

rricploic 

-fn,cnl 

+ 

fJ 

+ 

fn,jn) 

mja‘j 

—  fn,cn^ 

+ 

+ 

= 

mja^ 

t,c|rj  X  n°\ 

+ 

+ 

= 

mjpjaj. 

In  the  presence  of  friction,  there  also  exist  tangential  reaction  forces.  Let  their 
magnitudes  be  /*  and  /j.  The  are  subject  to  the  restrictions 

0  ^  1/el  —  ^cfn,c 

0  ^  \fj\  ^  Mj/n.j- 

Observe  that  the  sliding  tangents  in  real  space  are  (n*,— nj)  and  (nj,—nj). 
As  in  [E],  let 

vS  =  n*rf  +  n?r» 


v]  =  njrJ  + 

Then  in  the  presence  of  friction,  the  equations  of  motion  are 


<V.c 

fc 

/n,cn*  +  /*n» 

+ 

= 

/n,c«f  -  fc^c 

+ 

Pc 

=  mcO? 

fn,cnl  +  flvt 

+ 

Tc 

= 

/n,cn?  +  /!n 


+  +  fjn 

+  fnjn^j  -  f]n 

+  fn,jn]  +  /ju 


In  eq.  (iZl),  we  have  indicated  with  braces  which  terms  correspond  to  which 
generalized  applied  forces  and  accelerations.  They  must  be  normalized  relative  to 
the  inner  product;  this  is  accomplished  by  dividing  the  torque  equations  by  pc  and 
pj,  respectively.  For  example,  the  torque  component  of  is  of  course  actually 
fl  =  j^Tc.  Me  and  Mj  are  generalized  mass  matrices,  combining  the  mass  and  the 
moment  of  inertia  of  the  moving  objects. 

We  will  consider  the  case  where  contact  is  not  broken.  That  is,  the  normal 
accelerations  are  zero.  Henceforth,  we  will  adopt  the  standard  dot  notation  for  the 
inner  product  on  the  teingent  space.  Writing  this  out, 

Ac  •  He  =  0 

A  n  (^2) 

Aj  •  nj  =  0. 

Now,  attach  an  inertial  reference  frcime  £  to  H  at  the  reference  point.  L 
accelerates  by  Aj.  The  robot  accelerates  at  Ad  therefore  in  L  it  accelerates  at  “the 
svun^**  of’  Ac  and  —  Aj.  The  acceleration  of  L  generates  a  fictitious  force  h;  we  can 
write  the  effective  applied  force  in  C  relative  to  L  by  adding  the  ficticious  force  h 
arising  from  the  acceleration  of  L: 


C  =  U  +  h 


{R3a) 


In  general,  h  will  be  complicated,  since  L  is  &  rotating  coordinate  system.  It  is 
conceptually  simple,  however.  For  example,  in  the  case  of  no  rotations,  we  simply 
have  h  =  — McAj. 

The  reaction  force  fr,c  may  be  found  by  projecting  the  effective  applied  force 
f^  onto  the  Erdmannian  friction  cone  Kx-  The  global  reference  frame  serves  as  a 
non-inertial  frame  for  the  table;  hence  the  effective  apphed  force  on  B  is  simply 


^'  =  fj  +  C-  (i236) 

The  reaction  force  fr j  may  be  found  by  projecting  the  effective  applied  force 
onto  the  Erdmannian  friction  cone  Ka- 

[E]  derives  two  canonical  tangent  vectors  tr  and  t^.  tp  is  the  tangent  in  the 
direction  of  pure  rotation  about  the  point  of  contact.  It  is  normal  to  the  plane  of 
the  Erdmannian  friction  cone.  The  reaction  force  may  be  found  by  projecting  the 
effective  applied  force  along  tr  onto  the  plane  of  the  friction  cone.  If  the  projection 
lies  inside  the  friction  cone,  then  the  projection  is  the  reaction  force.  If  not,  then  we 
must  project  perpendicularly  along  t^,  in  the  plane,  onto  the  edge  of  the  friction 
cone.  The  second  projection,  then,  is  the  reaction  force. 

Now,  let  ti  and  t;2  be  two  orthogonal  vectors  in  the  plaice  of  such  that 
ti  =  — t^.  Then  the  projection  onto  the  plane  of  Kj.  is  given  by 

^°Before  adding  the  generalized  forces,  the  angular  accelerations  must  be  resolved  relative 
to  the  different  rotating  coordinate  systems. 


f;=  iR4) 

This  yields  two  cases.  In  the  first,  the  projection  lies  in  the  friction  cone,  so 


fr.c  =  fc.  (i25) 

In  the  second,  we  must  project  again.  That  is,  is  the  projection  of  fc  along 
onto  the  boundary  of  Kg.  In  the  latter  case,  we  can  express  the  second  projection  in 
local  (ti,t2)  coordinates  as  follows.  Write  fc  =  — (/i,/2)  in  local  coordinates.  Call 
the  projection  p  =  (pi,P2)-  Let  e  =  (61,62)  he  a  unit  vector  along  the  edge  of  the 
friction  cone  Kx,  expressed  in  local  coordinates.  We  obtain  a  quadric  constraint. 


P2  =  f2 


Pi  Cl  +  P2C2  = 


(R6) 


Analogously,  we  have  that  frj  is  related  to  fj  by  a  projection  in  Tq  onto  the 
plane  of  Ka-  Thus  it  is  possible  to  derive  three  more  constraints  (R4'),  (R5'),  and 
(i?6')  in  precisely  the  same  manner. 

The  (R)  equations  may  then  be  solved  simultaneously  to  predict  the  reaction 
forces  and  resulting  motion. In  considering  this  analysis,  note  the  crucial  role 
played  by  the  geometrical  projections  onto  the  finction  cone.  It  remEuns  to  gener- 
ahze  the  analysis  to  consider  multiple  points  of  contact,  breaking  contact,  coriolis 
forces,  and  centripital  accelerations.  I  expect  that,  given  the  tools  developed  above, 
the  generalization  should  follow  [E]  straightforwardly.  However,  I  also  expect  the 
resulting  system  of  constraint  equations  to  remain  fairly  complicated,  as  illustrated 
above.  Since  the  planner  must  solve  this  system,  a  simpler  method  for  computing 
reaction  forces  would  be  desirable.  The  techniques  above  require  algebraic  manip¬ 
ulation  as  well  as  geometric^d  computation.  There  may  be  simplifying  £issumptions 
which  feicilitate  the  computation.  This  direction  must  be  explored  in  order  to  build 
a  practical  planning  system. 


Quasi'Static  Analysis 

One  simplifying  approach  is  quasi-static  analysis.  Such  an  analysis  would  proceed 
as  follows.  First,  all  accelerations  in  equations  (i?l)  could  be  set  to  zero.  The 
reaction  force  can  be  in  one  of  three  states:  on  the  “left”  edge  of  Kx,  inside  Kx, 
or  on  the  “right”  edge  of  Kx-  The  magnitude  of  fp^c  can  be  determined  from  the 
normal  component  of  fc,  since  the  accelerations  are  zero.  Each  state  corresponds 
to  different  impending  motions  in  the  direction  of  the  force  imballance.  (It  could 
also  correspond  to  sticking  on  the  surface  of  B).  Similarly,  there  are  three  possible 

**0f  course,  motion  ambiguities,  as  in  [E],  are  still  possible. 


states  for  frj,  yielding  six  qualitative  states  altogether.  One  envisions  an  algorithm 
as  follows.  The  algorithm  hypothesizes  a  state,  say,  that  fr,c  lies  on  the  left  edge  of 
Kx-  This  means  that  it  can  be  found  from  fc  via  the  one  or  two  step  Erdmannian 
projection  given  in  (i24-i26).  Note  that  there  are  no  fictitious  forces,  since  the 
accelerations  are  zero;  hence,  for  example,  in  the  easy  case  we  simply  have 

fr,c  =  — (fc  •  ti)ti  ~  (fc  ■  ^2)^2-  (■^4) 

Next  the  algorithm  makes  a  hypothesis  about  the  state  of  the  reaction  force  frj- 
This  results  in  one  or  more  hypothesis  equations  like  {H4:).  If  equations  (i21)  and 
the  hypothesis  equations  are  consistent,  then  the  hypothesized  reaction  forces  are 
possible  imder  quasi-static  assumptions.  Furthermore,  associated  with  the  hypoth¬ 
esized  reaction  forces,  there  is  a  (set  of)  impending  motions,  corresponding  to  the 
resultant  of  the  force  imbalJance.  These  impending  motions  may  be  used  to  predict 
the  effects  of  applied  forces  under  quasi-static  assumptions. 

The  advantage  of  this  method  may  lie  not  only  in  its  simplicity,  but  in  the  fact 
that  it  gives  a  partition  of  force  space  into  impending  motion  regions.  Forces  applied 
in  a  given  motion  region  will  result  in  motions  in  a  particular  (set  of)  direction(s). 
Such  a  technique  could  be  very  useful  in  a  planner  for  pushing  operations.  The 
investigation  of  simplifying  assumptions  such  as  quasi-static  analysis  is  a  fruitful 
direction  for  further  research. 

Application  to  Planning 

Suppose  that  it  is  impossible  to  exert  forces  at  the  center  of  mass  of  B,  Thus 
the  control  velocity  uncertainty  cones  lies  entirely  in  the  tangent  spaces  to  C,  and 
contain  no  component  across  J.  Any  motion  of  B  must  be  effected  by  the  transferred 
pushing  force.  Under  generalized  damper  dynamics,  the  tools  above  are  sufficient 
to  characterize  all  possible  resulting  motions  from  a  cone  of  applied  forces.  Thus  we 
can  define  forward  projections,  backprojections,  and  preimages  in  Q  when  motion 
across  J  due  to  pushing  is  possible.  Motion  across  J  is  only  posible  on  certain 
surfaces;  it  is  impossible  to  move  across  J  in  free-space. 

Imagine  a  backchaining  planner  in  a  generalized  configuration  space  endowed 
with  this  physics.  Such  a  planner  could  compute  motion  strategies  which  may  be 
characterized  as  follows: 

•  In  free  space,  or  on  surfaces  generated  by  immovable  objects,  all  differential 
motions  lie  within  one  o-slice. 

•  Along  surfaces  generated  by  objects  that  can  be  pushed,  the  differential  motions 
are  tangent  to  the  surface  in  Q,  and  may  move  across  J  as  well  as  C. 

The  resulting  motion  strategy  consists  of  a  sequence  of  gross  motions,  compliant 
motions,  and  pushing  motions. 


Generalized  Control  Uncertainty  as  a  Non-holonomic  Constraint 

Suppose,  as  above,  it  is  impossible  to  exert  forces  at  the  center  of  mass  of  5,  so 
the  generalized  control  uncertainty  in  Q  has  no  component  in  the  tangent  space  to 

J .  (See  fig.  11.19.)  Consider  example  (1)  when  B  has  three  degrees  of  freedom  and 
the  robot  has  two.  While  the  tangent  space  to  Q  is  five  dimensional,  note  that  in 
general,  the  forward  projection  of  a  point  will  be  of  lower  dimension.  For  example, 
from  a  point  in  free  space,  the  forward  projection  will  lie  in  a  2D  shce  of  Q.  From 
a  point  on  a  surface  in  the  forward  projection  will  typically  lie  on  the  surface. 

This  is  a  subtle  and  deep  point.  The  constraints  in  most  motion  planning 
problems  [Lozano-Perez,  SS,  D,  E,  C]  are  constraints  on  the  degrees  of  freedom  of 
the  moving  objects.  Such  constraints  are  called  holonomic  constraints;  they  can 
be  expressed  as  constraint  surfaces  in  the  configuration  space.  However,  the  gen¬ 
eralized  control  imcertainty  and  the  characterization  of  friction  express  constraints 
not  on  the  degrees  of  freedom  of  the  object,  but  on  its  differential  motions.  Infor¬ 
mally,  this  is  clear  since  in  fig.  11.19,  is  2D,  while  the  tangent  space  is  5D. 

Such  constraints  are  Ccdled  non-holonomic  constraints;  they  can  not  in  general  be 
expressed  as  constraint  surfaces  in  the  configuration  space.  They  can  be  character¬ 
ized  as  constraints  in  the  tangent  bundle,  as  in  the  sticking  computation  employing 

K.  Computing  backprojections  under  non-holonomic  constrmnts  requires  enforc¬ 
ing  the  differential  motion  constraints  as  well  as  the  usual  holonomic  constraints 
imposed  by  surfaces  in  generalized  configuration  space. 

§A.5.  A  More  Formal  Summary  of  the  Construction  of  H 

We  now  summarize  the  construction  of  phase  space  EDR  regions  somewhat 
more  formally.  We  construct  H  as  follows.  Given  a  motion  strategy  a  goal 
region  G,  and  a  start  region  R  in  generalized  configuration  space  ,  we  construct: 

F  =  Fe{R) 
p  =  PfAG) 
p  =  PfAG) 

Hs  =  {  X  ^  P  -  P|  sticking  can  occur  at  x  } 

Z(ff,)  =  {(x,i;)e7r-‘(H,)|i;  =  0} 

H  =  ir-\F-P)(jZiH,). 

The  forward  projection  F  is  in  position  space,  not  phase  space.  The  map  Z  is 
the  zero  section  of  generalized  configuration  space  . 

Now,  given  a  collection  of  goals  {  }  we  denote  their  backprojection  under 

a  commanded  velocity  uj  by  Bg{{G0  }).  Note  this  is  equal  to  Bg{[jg  GAi  since 
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backprojections  do  not  address  recognizability.  From  the  construction  of  H,  we 
have 


Bei{G,nH})DR.  (7) 

(7)  is  a  reachability  consequence  only.  To  form  an  EDR  strategy  using  H,  we  must 
add  a  recognizability  constraint  analogovu.  to  eq.  (3).  We  allow  simple  goals  in 
phase  space;  that  is,  we  permit  goals  in  phase  space  as  arguments  to  Pr,9.  The 
recognizability  constraint  is  then 

=  (3«) 

As  written,  (3a)  is  a  reachability  and  recognizability  constraint.  But  since 
reachability  (7)  follows  from  the  construction  of  H,  (3a)  adds  exactly  recognizability 
to  the  construction. 

More  generally,  we  could  replace  G  throughout  by  a  collection  of  goals  {  G^  }. 
(3a)  then  becomes  the  obvious 

P«.fl({7r-^G^)}U{H})  =  R. 

§A.6.  Definition  of  an  Approximate  Push-Forward 


Here  is  the  formal  definition  of  one  kind  of  push-forward.  (There  are  other 
kinds).  This  push-forward  is  obtained  by  “lying”  to  the  termination  predicate  about 
where  the  motion  started.  It  captures  the  intuitive  notion  of  “trying  the  strategy 
anyway,  even  if  we’re  not  guaranteed  to  be  in  the  right  initial  region.” 

Suppose  we  have  a  motion  6  from  a  region  R  which  achieves  some  set  of  goals 
{  Gp  }.  That  is, 

PrA{  G^})  =  R. 

Let  U  be  an  arbitrary  region  in  generalized  configuration  space  .  For  example,  we 
might  not  be  guaranteed  to  have  the  initial  position  within  R\  it  might  lie  within 
some  region  5.  We  let  C/  =  S  —  R,  and  investigate  the  effect  of  executing  the 
strategy  from  U  while  lying  to  the  termination  predicate:  we  tell  it  we  were  really 
within  R  when  the  motion  started. 

We  define  a  push-forward  of  6  from  V  with  respect  to  R  and  {  Gfi  },  denoted 
F,(^,  U,R,  {  G^  }),  as  follows: 

We  assume  a  termination  predicate  with  no  local  history  and  without  time: 
see  [Erdmann].  The  formulation  for  variations  of  this  predicate  are  very  similar. 
Forward  projections  in  phase  space  are  denoted  with  tildes.  Recall  n  is  the  canonical 
projection  of  phase  space  onto  configuration  space.  Given  Ein  actual  position  and 
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velocity  a  corresponding  sensed  position  and  velocity  is  denoted  (x*,v*). 

is  said  to  be  consistent  with  (x,t;)  iff  (x*,u*)  €  Bep(x)  x  Bev(v). 

F^{6,  U,  R,  {  })  is  the  set  of  pairs  (x,  v)  £  Fo{U)  such  that  there  is  an  initial 

position  po  E  U  and  a  corresponding  sensed  position  pj  ^  5ep(Po)niinPf,(/j)_^(G), 
and  some  G  E  {  G^  },  such  that  for  all  {x*,t;*)  consistent  with  (x,  u), 

5ep(x*)  X  BUv*)  n  Fs{R)  c  ir-\G).  {*) 

Note,  however,  that  it  is  possible  for  the  motion  not  to  terminate.  For  the  saJce 
of  discvission,  however,  assume  it  does  terminate,  either  by  tising  velocity  thresh¬ 
olding  or  time.  This  requires  indexing  the  forward  projection  in  (*)  by  time.  These 
assumptions  allow  us  to  prove  some  lemmas  that  provide  some  intuition  about  this 
piish  forward. 

Now,  suppose  further  that  R  C  S.  Abbreviate  F»  =  7rF*(^,  S,  R,  {  G^  })).  The 
following  lemmas  help  characterize  the  push  forward  F*.  Let  H  be  the  EDR  region 
as  given  in  (6),  and  let  H  =  irH.  F*  says  exactly  where  the  strategy  0  will  terminate 
when  executed  from  5,  if  we  lie  to  the  termination  predicate  and  tell  it  we  really 
started  from  R. 

Lemma:  Be{F^)  D  R. 

Lemma;  Pii^g(F^)  =  R. 

Now,  in  general,  maximal  preimages  do  not  exist  [Mason,  E].  If  F  is  not  a 
majcimal  preimage,  then  the  equation 

F,  CFU(ljG^)  (♦♦) 

need  not  hold.  It  would  be  useful  to  prove  or  disprove  the  following: 

Conjecture;  (**)  holds  when  R  is  majcimal. 

In  particular,  if  there  is  just  one  goal  G,  then  this  would  imply  F,  C  F  U  G. 

§A.7.  The  Formal  Requirements  for  Push-Forwards 


We  now  characterize  the  formal  requirements  for  push-forwards.  Note  that  the 
approximate  push-forward,  above,  need  not  satisfy  these  constraints. 

For  notational  purposes,  we  regard  motion  strategies  as  mappings,  and  so  we 
write 

Ri  =  PRi.eiiRi-l)  {*) 

as 


•« 
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If  the  Ri  are  successive  subgoals  in  a  plan  whose  motions  are  the  di,  then  (*)  is  a 
necessary  and  sufficient  condition  for  the  subgoals  to  be  suitable  for  back-chaining 
[LMT.] 

Given  as  data  strategy  a  goal  Ri-i,  an  actual  start  region  R,  and  a  region  R, 
from  which  Oi  is  guaranteed,  we  cam  always  construct  an  EDR  region  ffi-i. 
is  see  (6)).  Of  course  we  may  not  be  able  to  construct  an  EDR  strategy  from 

Hi-i.  Denote  the  dependence  of  H  on  its  data  by 

Hi.i=H{R,ei), 

so  we  imagine  examining  the  “domain”  and  “range”  of  Oi  to  obtain  Ri  and  i2,_i. 
If  there  exists  an  EDR  strategy  for  the  EDR  region  we  denote  this  by 

Ri-i 

R 

S 

H{R,9i). 

More  generally,  we  could  replace  with  the  distinguishable  union  of  some  set 
of  goals.  As  suggested  in  the  informal  exposition,  we  will  in  fact  replace  it  with 
the  set  of  unattained  subgoals  in  an  n-step  plan.  The  motion  $i  must  then  achieve 
either  some  unattained  subgoal,  or  the  EDR  region. 

Now,  suppose  we  are  given  a  guaranteed  n-step  plan,  ©, 

=  G. 

Define  TZj  =  {  ,  Rj,  JJq  |  to  be  the  distinguishable  union  of  all 

unattained  subgoals  (after  Rj).  The  one  step  EDR  strategy  we  seek  may  achieve  any 
one  of  these  subgoals.  Suppose  the  start  region  R  contains  Rn-  ^^e  can  construct  an 
n-step  EDR  strategy  with  start  region  R,  (using  0  as  data)  if  there  exist  termination 
regions  F,„, . . .  ,F,„_i,  for  some  i  between  1  and  n,  such  that 


I  ^ 

F.  Rn-l  U  Hn-1 

where 


In  other  words, 


such  that 


F*n  =  i2 

F»n-1  C  Rn-l  U  HiRy  ^n) 

F^n-2  C  Rn-2  U  -ff (■F’*n-1 »  ^n-1 ) 

F^n—x  C  Rrt—x  U  i+1 »  ^n— i 


j  =  Ff.  j  ,ej  (F,  j- 1 )  for  all  j, 
and  such  that  there  exists  a  one-step  EDR  strategy 


Rn-i-\ 


F *n—i 


^n-<  H{F,n_i,6n-i)- 

These  termination  regions  {  j  }  characterize  the  requirements  for  push-forwards. 
The  push-forward  should  be  a  function  satisfying  these  constraints,  by  which  we 
mean  that  the  push-forward  is  a  set-valued  map  whose  values  are  these  termination 
regions.  We  hope  that  the  termination  regions  may  be  approximated  by  push- 
forwards  such  as  the  example  in  the  last  section.  Computing  exact  push-forwards 
appears  to  be  at  least  as  hard  as  solving  n-step  pre-image  equations. 

Note  (1):  When  maximal  preimages  do  not  exist,  for  completeness  it  may  be  nec¬ 
essary  to  employ  a  weaker  constraint  on  termination  regions.  For  example,  the 
weakest  constraint  would  be 

HiF.j,9j)  =  Fej{F,^). 

See  the  previous  section  for  more  details. 

Note  (2):  As  the  notation  suggests,  it  is  possible  to  formalize  the  view  of  “mo¬ 
tions  as  mappings” — this  notion  is  implicit  in  the  term  “preimage.”  To  develop  this 
viewpoint,  one  considers  motions  as  a  certain  cl^lss  of  morphisms  between  distin¬ 
guishable  unions  in  the  powerset  of  the  tangent  bundle  to  generalized  configuration 
space  .  An  EDR  theory,  then,  is  a  covariant  functor  associated  with  a  family  of 
quotient  maps  of  the  form 

i,-.TCxJ^{P,P-P,F-P]. 

While  it  is  possible  to  push  such  '  functorial  viewpoint,  any  category-theoretic 
formulation  of  this  flavor  will  almost  certainly  be  exclusively  descriptive. 
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(LET  ( (CURRENT -EVENT  (AVL-ACCESS  EVENT  TREE  »'EVENT->))) 

(COND  (CURRENT-EVENT 

(SETF  (COLOR-CHANGE  CURRENT-EVENT)  (ADD-COLORS  (COLOR-CHANGE  EVENT) 

(COLOR-CHANGE  CURRENT-EVENT) ) ) 

294 


(ir  (rHTE?  ICOLOB-CHANta:  COBIIEKT-EVENT'  > 

(AVL-DELETE  CUlWEHT- EVENT  TREE  »•  EVENT ->  NO-NUTATIONi 
TREE) ) 

(T  (AVL-INSERT  EVENT  TREE  #  EVENT->  NO-M'JTATIONI  i  i  i  i 


IDEEUN  EVENT-SOB  (EVENT  TREE  40PTI0IIAL  NO-MOTATION) 

(EVENT-ADD  (HAKE-EVENT  POINT  (POINT  EVENT)  EON  (EON  EVENT) 

COLOR-CHANGE  (NEC-COLOR  (COLOR-CHANCE  EVENT))) 
TREE  NO-MUTATION) ) 


(DEFUN  OLO-MERGE-EVENTS  (A  B) 

(DO*  ((START  (NCOMS  NIL)) 

(PTR  START)  ) 

(  (NOT  (AND  A  B)  ) 

(SETT  (REST  PTR)  (OR  A  B) ) 

(REST  START) ) 

(LET  ((SIGN  (SLOPE->  (EQN  (FIRST  A) )  (EON  (FIRST  B) ) ) ) ) 

(COND  (  (PLUSP  SIGN) 

(SETT  {REST  PTR)  (NCONS  (FIRST  B) ) ) 

(SETQ  PTR  (REST  PTR) ) 

(SETQ  B  (REST  B) ) ) 

( (MINUSP  SIGN) 

(SETT  (REST  PTR)  (NCONS  (FIRST  A) ) ) 

(SETQ  PTR  (REST  PTR)  ) 

(SETQ  A  (REST  A)  )  ) 

(T  (LET  (  (NEN-COLOR  (ADD-COLORS  (COLOR-CHANGE  (FIRST  A)  ) 

(COLOR-CHANGE  (FIRST  B) ) ) ) ) 

(COND  ( (NOT  (FREE?  NEN-COLOR) ) 

(SETT  (REST  PTR)  (M<»HS  (MAKE-EVENT  POINT  (POINT  (FIRST  A)  ) 

EQN  (EQN  (FIRST  A) ) 
COLOR-CHANGE  NEN-COLOR) ) ) 

(SETQ  PTR  (REST  PTR)))) 

(SETQ  A  (REST  A)  B  (REST  B) ))))))  ) 


I  da  fun  Mrga-avanta  (a  b) 

(do*  ( (atare  (ncona  nil)) 

(ptr  atart)) 

( (noe  (and  a  b)  ) 

(aatf  (raat  ptr)  (or  a  b) ) 

(raat  atart) ) 

(lot  ((aign  (slop«->  (aqn  (flrat  a))  (aqn  (flrat  b) ) ) ) ) 

(eand  ( (pluap  mlqn) 

(aatf  (raat  ptr)  (ncona  (flrat  b) ) ) 

(aatq  ptr  (raat  ptr)) 

(aatq  b  (raat  b) ) ) 

( (nlnuap  sign) 

(aatf  (raat  ptr)  (neons  (first  a))) 

(aatq  ptr  (raat  ptr) ) 

(aatq  a  (raat  a))) 

(t  (lat  ( (naw-color  (add-eolors  (color-ehanga  (first  a) ) 


(color-changa  (first  b) ) ) ) ) 

(cond  ((not  (fraa?  naw-color)) 

(aatf  (raat  ptr)  (neons  (ina)ce-evont  point  (point  (first  a)) 

aqn  (morga-aquatlons 
(oqn  (first  A) ) 

(aqn  (first  B)  )  ) 
color-c)ianga  naw-color)  ) ) 


(aatq  ptr  (raat  ptr)))) 
(aatq  a  (root  a)  b  (raat  b) )))))) ) 


;;  Marga  tha  ganarators  of  two  awants.  Assumo  t))ac  Eqnl  and  Eqn2  ara  aqual. 
;  T))ay  must  both  hava  obatacla  tags. 


(dafun  marga-aquatlons  (Eqnl  Eqn2) 

(cond  ((and  (ganarators  (propartlaa  Eqnl)) 

(ganarators  (propartlaa  Eqn2) ) 

(not  (aqual  (ganarators  (propartlaa  Eqnl)) 

(ganarators  (propartlaa  Eqn2) ) ) ) ) 

(mata-aqn  X  (x  Eqnl)  Y  (Y  Eqnl)  D  (D  Eqnl) 

Propartlaa  (ma)(s-adga-dascrlptlon  Tag  (tag  (propartlaa  Eqnl)) 


(T  Eqnl) ) ) 


Ganarators 

(marga-ganaratora 

(ganarators  (propartlaa  Eqnl)) 
(ganarators  (propartlaa  Eqn2) ) ) ) ) ) 


•• 

V.  •r,  -r.  -'.V'  V.  r.  ■' 

'.-V,  V  .  .V.V  ■.•.V.'.','--  -iSr7;v V .-.•v 


I  or »  <  Z'.  G2  ) 

f  •pp^na  !i*  iCO-v«rt»«-p  G'.i  (Hat  31i  SI) 

t  If  ■  "'O-vart  aK-p  S2 )  (*lat  S2)  G2 )  )  > 

;ErUK  AVT.-HtAD-VlJ.  lEVZHT -QUEUE) 

:_ET  iriRST-EVEMT) 

(HUETIPLE-VALUE  (riMT-EVERT  EVENT-QUEUE) 

AVL-HEAO  EVENT-QUEUE)  ) 

:r  rmsT-EVENT 

!DO*  (  (EVENTS  (NCONS  EIRST-EVENT)  ) 

(PTR  EVENTS  (REST  PTR) I  I 
1 (NULL  PTR)  (VALUES  EVENTS  EVENT -QUEUE) ) 

(LET  ( (NEXT -EVENT  (LETT-MOST  EVENT -QUEUE) ) ) 

(COND  ( (AND  NEXT-EVENT  (EQUAL  (POINT  NEXT-EVENT)  (POINT  TIRST-EVENT) ) ) 
(MULTIPLE -VALUE  (NEXT-EVENT  EVENT-QUEUE) 

(AVL-KEAD  EVENT-QUEUE) ) 

(SETT  (REST  PTR)  (NCONS  NEXT-EVENT ) ) ) ) ) ) 

(VALUES  NIL  NIL)  )  )  ) 


ZZfJti  FILTER  iLIST  PREDKLATE) 
zr.z  I  (OUTPUT  NIL)  ) 

(DOLIST  (EL  LIST) 

(IF  (FUNCALL  PREDICATE  (COLO*  EL)) 

(SETQ  OUTPUT  (CONS  EL  OUTPUT)))) 

OUTPUT) ) 

OETUN  SWEEP-UNION  (POLYS)  ; ravarsa  my  loopa  aoon ( 

iFILTER  (PROJECT  POLYS  ()  0  0)  ♦TREE?)) 

OEFUN  LOCATE-POINT  (POINT  QUERY-TREE) 

LET  ( (F-POINT  (MAKE-POINT  X  (FLOAT  (X  POINT))  Y  (FLOAT  (Y  POINT))))) 
(AVL-ACCESS  POINT 

(INTERVALS  (AVL-ACCESS  (X  POINT)  QUERY-TREE  ♦TN-RANOE?)  ) 

♦  '  (LAMBDA  (A  B)  (IN-INTERVAL?  A  B  F-POINT))))) 

DErUN  LOCATE-X-Y  (X  Y  QUERY-TREE) 

LOCATE-POINT  (MAKE-POINT  X  X  Y  Y)  QUERY- TREE I ) 


aafur  SWEEP  (polyt  toptional  ^ary) 

(projaet  polya  nil  I\2  -1\2  0.0  quary) )  j 

jaagra:  (OUTER-UNION  (SWEEP  INPUT))  ate  J 

aafun  oucar-vinion  (polya)  , 

mapear  ♦ '  coaiplamant -poly  (filtar  polya  ♦'{raa?)))  j 


TANGENT  la  tha  alopa  of  tha  v.c.  adqa. 

for  backpro^action,  tha  top  and  bottom  tanpanta  ara  of  tha  form 
(  mlnua  alopa  ,  poaitlva  alopa) 

;;  call  with  arga  Ilka:  (backpro jactlon  anvlronmantl  atart-roQlonl  IMO  .26) 
dafun  forward-pro jact ion 

(obataclaa  atart-raQion  tanQant  mu  toptional  (juary) 

(lot  (  (top-tanQont  (aba  tariQant)) 

(bottom-tangant  (-  (aba  tanoant)))) 

(pro jact  obataclaa  atart-ragion  top-tangant  bottom-tangant  mj  quary) ) ) 


daf'jn  waak -backpro jactlon 

(obataclaa  atart-ragion  tangant  mu  toptlonal  guary) 
(lot  (  (backprejaction?  ’waak) 

(top-tangant  (aba  tangant)) 

(bottoat-tangant  (-  (aba  tangant)))) 

(lot  ( ( *nu*  mu) 

( float 
(float 
(float 
(float 


(•nag-vl*  (Hat 


(*naq-v2*  (Hat 


(danonlnator  bottom-tangant)) 
(numarator  bottom-tangant)))) 
(danoaiinator  top-tangant)) 
(numarator  top-tangant))))) 


(pro jact  obataclaa  atart-ragion  top-tangant  bottom-tangant  niu  quary)))) 


dafvar  “mu'l 
dafvar  •nag-vl*) 
dafvar  *nag-v2*) 
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*m  ./.a-  -•  . 

a  ’‘_a  •  .  •*  /•>'■  a"*  A  • 


•  aVA. 


^  .o.-t-'  u,  .«-’i  ..i-aAAk.\  .  O. .t-* 


[i 


<cl«tfun  b«ekprojactlon  (OBSTACI^S  STAMT-REGION  TANGENT  HU  tOPTZONAL  QUERY) 
(l«c  ( (baeicpro  jactlon?  '  '  atrong) 

(top-cangant  (-  (aba  Cangant))) 

(botcon-cangant  (aba  tangant))) 

(lat  ( (*mu*  mu) 

(*nag-vl*  (Hat  (float  (danonlnator  top-tangant) ) 

(float  (nuBwrator  top-tangant)))) 

(*nag-v2*  (Hat  (float  (danoeU.nator  bottom-tangant) ) 

(float  (nuaMrator  bottom-tangant))))) 


(projact  OBSTACLES  START-REGION  TOP-TANGENT  BOTTOM- TANGENT  MU  QUERY)))) 


(dafmacro  bac)cpro jactlon?  () 

'  bac)cpro  jaet  1  on  7 ) 

(dafmacro  atrong-bac)cpro jaetlon?  () 

Mag  (bac)ipro  jaetlon?)  '  atrong) ) 

(dafmacro  waa)c-bac)cpro jaetlon?  () 

Mag  (backpro jaetlon?)  'waak)) 

(OEFUN  PROJECT  (OBSTACLES  START-REGION  TOP-TANGENT  BOTTOM-TANGENT  MU  (OPTIONAL  QUERY) 
(LET*  ((SA  (MHEN  MU  (//  MU  (SORT  (1+  (•  MU  MU) ) )  )  )  ) 

(EVENTS  NIL) 

(STRUCT  (MAKE-OUTPUT-STRUCTURE  POLYGON  (MAKE-POLYGON  COLOR  0) ) ) 

(FREE-INTERVAL  (?(AKE- INTERVAL  TOP  (MAKE-EQN  X  0  Y  1) 

BOTTOM  <MAKE-E(BI  X  0  Y  1) 

OUTPUT-STRUCTURE  STRUCT) ) 

(SWEEP-LINE  (AVL- INSERT  FREE-INTERVAL  NIL  NIL)  ) 

(EVENT-QUEUE  (QUEUE-INITIAL-EVENTS  OBSTACLES  'OBSTACLE  FREE-INTERVAL 

(QUEUE-IMITIAL-EVENTS  START-REGION  'START  FREE-INTERVAL  NIL))) 
(OLO-STRUCTS  NIL) 

(QUERY-TREE  NIL) ) 

(SETT  (TOP-END  STRUCT)  STRUCT) 

(SETF  (BOTTOM-EML  STRUCT)  STRUCT) 

(MULTIPLE-VALUE  (EVENTS  EVENT-QUEUE)  (AVL-HEAD-ALL  EVENT-QUEUE)  ) 

(DO  (  (POLYS  NIL) 

(OLO-X  NIL)  ) 

( (NOLL  EVENTS) 

(COND  (QUERY 

(SETQ  QUERY-TREE 

(AVL-INSERT  (MAXE-QOlWY-NOOE  X-MIN  OLO-X  INTERVALS  SMEEP-LINE) 
QUERY-TREE  •'ALNAYS->)) 

(FIX-OLO-STROCTS  OLO-STRUCTS) > ) 

(VALUES  (CONS  (POLYGON  (OUTPUT-STRUCTURE  (VALUE  SMEEP-LINE)))  POLYS) 

QUERY-TREE) ) 

(LET*  ( (POINT  (POINT  (FIRST  EVENTS) ) ) 

(F-POINT  (MAKE-POINT  X  (FLOAT  (X  POINT)  )  Y  (FLOAT  (Y  POINT) ) ) ) 
(RELEVANT-INTERVALS  (AVL-ACCESS-ALL  POINT  SWEEP-LINE 

*'  (LAMBDA  (A  B) 

(IN-INTERVAL?  A  B  F-POINT)))) 

(INT->-  #' (LAMBDA  (A  B)  (IMTERVAL->  A  B  POINT  F-POINT  -1))) 

(INT->4-  •'  (LAMBDA  (A  B)  (ZNTERVAL->  A  B  POINT  F-POINT  1)))) 

(COND  ((AND  QUERY  (OR  (MULL  OLO-X)  (#  OLO-X  (X  POINT)))) 

(SETQ  QUERY-TREE  (AVL-INSERT  (MAXE-QUERY-NODE  X-MIN  OLO-X  X-MAX  (X  POINT) 

INTERVALS  SWEEP-LINE) 
QUERY-TREE  *'ALNAYS-») 

(SETQ  OLO-X  (X  POINT)))) 

(IF  (NULL  (EQN  (FIltST  EVERTS) ) )  (SETQ  EVENTS  (CDR  EVENTS)  ) ) 

(SETQ  EVENTS  (MERGE-EVENTS  EVENTS  (CROSSING-EVENTS  RELEVANT-INTERVALS))) 
(MULTIPLE-VALUE  (POLYS  OLO-STRUCTS) 

(CLOSE-INTERVALS  RELEVANT-INTERVALS  POINT  POLYS  OLO-STRUCTS  QUERY) ) 
(MULTIPLE-VALUE  (RELEVANT-INTERVALS  SWEEP-LINE) 

(OEIXTE-INTERVALS  RELEVANT-INTERVALS  INT->-  SWEEP-LINE  QUERY) ) 

(MULTIPLE-VALUE  (SWEEP-LINE  EVENT-QUEUE  EVENTS  POLYS  OLD-STRUCTS) 

(UPDATE-INTERVALS  RELEVANT-INTERVALS  EVENTS  TOP-TANGENT  BOTTOM- TANGENT  SA 
POINT  INT->-  INT->+  SWEEP-LINE  EVENT-QUEUE  POLYS 
OLO-STRUCTS  QUERY)) 

(SETQ  SWEEP-LINE  (NEW- INTERVALS  RELEVANT-INTERVALS  EVENTS 

POINT  INT->+  SWEEP-LINE  QUERY))) 

(MULTIPLE-VALUE  (EVENTS  EVENT-QUEUE)  (AVL-HEAD-ALL  EVENT-QUEUE) ) ) ) ) 

(DEFUN  QUEUE-INITIAL-EVENTS  (POLYS  TYPE  FREE-INTERVAL  EVENT-QUEUE) 

(DO  ((POLYS  POLYS  (REST  POLYS))) 

( (NULL  POLYS)  EVENT-QUEUE) 

(DOLIST  (LOOP  (LOOP-LIST  (FIRST  POLYS))) 


(OOLIST  (COGE  LOOP) 

iLET  (  (Y  (Y  (HEAD  EDGE)  )  ) 

(EON  (IF  (OR  (HINUSP  (Y  (EQM  EDGE))) 

(AND  (2EROP  (Y  (EON  EDGE)  )  > 

(PLUSP  (X  (EON  EDGE)  )  )  )  ) 

(NEG-EON  (EON  EDGE) ) 

(EON  EDGE)))) 

(IF  (OR  (NULL  (D  (TOP  FRZE-INTERVAL)  )  ) 

«  (-  -1  Y)  (D  (TOP  FREE-INTERVAL) ) ) ) 

(SETF  (0  (TOP  FREE-INTERVAL))  { - 1  Y) ) ) 

(IF  (OR  (NULL  (D  (ROTTOM  FREE-INTERVAL))) 

(>  (-  1  Y)  (0  (ROTTOM  FREE-INTERVAL)  )  )  ) 

(SETF  (D  (ROTTOM  FREE-INTERVAL))  (-  1  Y) ) ) 

(SETQ  EVENT-OOEDE  (EVENT-ADD  (MAKE-EVENT  POINT  (TAIL  EDGE) 

EON  EON 

COLOR-CHANGE  (ENCODE  TYPE  1)) 
EVENT-OUEUE  • '  EVBNT-> )  ) 

(SETO  EVENT-OUEUE  (EVENT-ADO  (MAKE-EVENT  POINT  (HEAD  EDGE) 

EON  EON 

COLOR-CHANGE  (ENCODE  TYPE  -1) ) 
EVENT-OUEUE  *' EVEMT->) >)))>) 


(DEFUN  CLOSE -INTERVALS  (INTERVALS  POINT  POLYS  OLO-STRUCTS  QUERY) 

(DO  (  (PTR  (REST  INTERVALS)  (REST  PTR) )  ) 

(  (NULL  (REST  PTR)  )  (VALUES  POLYS  OLO-STRUCTS)  ) 

(UNLESS  (FPT  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  (FIRST  PTR)  )  )  )  ) 
(MULTIPLE-VALUE  (POLYS  OLO-STRUCTS) 

(CLOSE-INTERVAL  (FIRST  PTR)  POINT  POLYS  OLD-STRUCTS  QUERY)  )  )  )  ) 


(OEFUN  CLOSE-INTERVAL  (INTERVAL  POINT  POLYS  OLD-STRUCTS  QUERY) 

(LET  ((STRUCT  (OUTPUT-STRUCTURE  INTERVAL))) 

(SETF  (TAIL  (FIRST  (TOP  STRUCT)))  POINT) 

(SETF  (BEAD  (FIRST  (BOTTOM  STRUCT)))  POINT) 

(CORO  ( (EQ  (BOTTOM-END  STRUCT)  STRUCT) 

(SETF  (LO(»-UST  (POLYGON  STRUCT)) 

(CONS  (TON  STRUCT)  (LOOP-LIST  (POLYGON  STRUCT) ) ) ) 

(IF  (HULL  (LOOP-END  (POLYGON  STRUCT))) 

(SETT  (LOOP-END  (POLYGON  STRUCT) )  (LOOP-LIST  (POLYGON  STRUCT) ) ) ) 
(SETO  POLYS  (CONS  (POLYOON  STRUCT)  POLYS))) 

(T  (SETF  (REST  (BOTTOM  STRUCT))  (TOP  STRUCT)) 

(SETT  (BOTTOM-END  (TOP-END  STRUCT)) 

(BOTTOM-ENO  STRUCT)) 

(SETF  (TOP-END  (BOTTOM-END  STRUCT)) 

(TOP-END  STRUCT)) 

(CONO  (QUERY 

(SETF  (POLYGON  STRUCT)  'DISCARDED) 

(SETO  OLO-STRUCTS  (CONS  STRUCT  OLO-STRUCTS))))))) 

(VALUES  POLYS  OLO-STRUCTS) ) 


IDEFUN  CROSSING-EVENTS  (INTERVALS) 

(DO  ( (THIS  (REST  INTERVALS)  (REST  THIS) ) 

(PREV  INTERVALS  THIS) 

(EVENTS  NIL)) 

(  (NULL  THIS)  EVENTS) 

(LET  ( (COLORl  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  (FIRST  THIS) ) ) ) ) 

(COLOR!  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  (FIRST  PREV)))))) 

(IF  (NOT  (OR  (AND  (FPT  COLORl)  (FREE?  COLOR!)) 

(AMO  (FPT  COLOR!)  (FREET  COLORl)))) 

(SETQ  EVENTS 

(CONS  (HAKE-EVENT  EON  (TOP  (FIRST  PREV)) 

COLOR-CHANCE  (COLOR-FIELD  (SUB-COLORS  COLORl  COLOR!))) 

EVENTS) ) ) ) ) ) 


(DEFUN  DELETE-INTERVALS  (INTERVALS  INT->-  SNEEP-UNE  QUERY) 

(DO  ( (PTR  (REST  INTERVALS)  (REST  PTR) ) 

(PREV  INTERVALS) ) 

((HULL  (REST  PTR))  (VALUES  INTERVALS  SNEEP-LINE) ) 

(COND  ((OR  (EQ  'DISCARDED  (POLYGON  (OUTPUT-STRUCTURE  (FIRST  PTR)))) 

(NOT  (FPT  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  (FIRST  PTR) )))))) 
(SETQ  SMEEP-UHE  (AVL-DELETE  (FIRST  PTR)  SNEEP-LINE  INT->-  QUERY)) 
(SETF  (REST  PREV)  (REST  PTR))) 

(T  (SETQ  PREV  PTR) ) ) ) ) 
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(OErUN  UPDATE-IHTERVXLS  (INTERVALS  EVENTS  TOP-TANGENT  BOTTOM-TANGENT  SA 

POINT  INT->-  INT->*  SWEEP-LINE  EVENT-QUEUE  POLYS  OLD-STRUCTS  QUERY) 

(LET*  ( (TOP-INTERVAL  (FIRST  (LAST  INTERVALS)  )  ) 

(MI DOLE- INTERVAL  (SECOND  INTERVALS)) 

(BOTTOM-INTERVAL  (FIRST  INTERVALS)  ) 

(TOP-TANGENT-EQN  (MAKE-EQN  X  (-  TOP-TANGENT)  Y  1 

D  (-  {•  (X  POINT)  TOP-TANGENT)  (Y  POINT) ) ) ) 
(BOTTOM-TANGENT-EON  (MAKE-EQN  X  (-  BOTTOM- TANGENT)  Y  1 

D  (-  (•  (X  POINT)  BOTTOM-TANGENT)  (Y  POINT))))) 

(SETQ  EVENTS  (PROJECTION-EVENTS  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  TOP-INTERVAL)  ) ) 

(COLOR  (POLYGON  (OUTPUT-STRUCTURE  BOTTOM- INTERVAL) ) ) 
TOP-TANGENT-EON  BOTTOM-TANGENT-EON  EVENTS  SA) ) 

(LET  ((TEMP-EVENTS  EVENTS)) 

(COND  ( (REST2  INTERVALS) 

(MULTIPLE-VALUE  (SWEEP-LINE  EVENT-QUEUE  TEMP-EVENTS  POLYS  OLD-STRUCTS) 
(UPDATE-KIDDLE-INTERVAL  TOP-INTERVAL  MIDDLE-INTERVAL  BOTTOM- INTERVAL 

EVENTS  TOP-TANGENT-EQN  BOTTOM-TANGENT-EQM  SA 
POINT  INT->-  INT->*  EVENT-QUEUE  SWEEP-LINE 
POLYS  OLD-STRUCTS  QUERY) ) ) ) 

(IF  (REST  INTERVALS) 

(COND  (TEMP-EVENTS 

(MULTIPLE-VALUE  (SWEEP-LINE  EVENT-QUEUE) 

(CHANGE-INTERVAL  TOP-INTERVAL  (TOP  TOP-INTERVAL) 

(EQN  (FIRST  (LAST  TEMP-EVENTS) ) ) 

POINT  INT->-  INT->-*  SWEEP-LINE  EVENT-QUEUE  QUERY)  ) 
(MULTIPLE-VALUE  (SWEEP-LINE  EVENT-QUEUE) 

(CHANGE-INTERVAL  BOTTOM- INTERVAL  (EON  (FIRST  TEMP-EVENTS)  ) 

(BOTTOM  BOTTOM- INTERVAL) 

POINT  INT->-  INT->*-  SWEEP-LINE  EVENT-QUEUE  QUERY) ) ) 

(T  (MULTIPLE-VALUE  (SWEEP-LINE  EVENT-QUEUE  OLD-STRUCTS) 

(JOIN-INTERVALS  TW-INTERVAL  BOTTOM-INTERVAL  POINT  INT->-  IMT->*- 
SWEEP -LINE  EVENT-QUEUE  OLD-STRUCTS  QUERY) ) ) ) 

(IF  TEMP-EVENTS  (MULTIPLE-VALUE  (SWEEP-LINE  EVENT-QUEUE) 

(SPLIT-INTERVAL  BOTTOM-INTERVAL  (EQN  (FIRST  (LAST  TEMP-EVENTS) ) ) 
(EQN  (FIRST  TEMP-EVENTS))  POINT 

INT->-  INT->*  SWEEP-LINE  EVENT-QUEUE  QUERY) ) ) ) ) ) 
(VALUES  SWEEP-LINE  EVENT-QUEUE  EVENTS  POLYS  OLD-STRUCTS)) 

(DEFUN  IN-BETWEEN  (BOTTOM-EVENT  MIDDUE  TOP-EVENT) 

(AND  (OR  (NULL  TOP-EVENT) 

(PLUSP  (SLOPE->  (EQN  TOP-EVENT)  MIDDLE))) 

(OR  (NULL  BOTTOM-EVENT) 

(PLUSP  (SLOPE->  lUODLE  (EON  BOTTOM-EVENT)))))) 

;;;  modlflwd  for  B«c)cpro jactlon  and  N«a)i  bacicpro joctlon 

(DEFUN  PROJECTION-EVENTS  (TOP-COLOR  BOTTOM-COLOR  TOP-TANGENT-EON  BOTTOM-TANGENT-EON  EVENTS  SA) 
(DO*  ( (NEW-CVENTS  (CONS  NIL  EVENTS)) 

(PTR  NEW-EVENTS  (REST  PTR) ) 

(COLOR  BOTTOM-COLOR) 

(OLD-CX)LOR  BOTTOM-COLOR)  ) 

( (NULL  PTR)  (REST  NEW-EVENTS) ) 

(LET*  ( (PREV-EVENT  (FIRST  PTR) ) 

(THI3-EVENT  (SECOND  PTR) ) 

(NEW-COLOR  (IF  THIS-EVENT 

(AOD-COLORS-CAREFULLY  COLOR  (COLOR-CHANGE  THIS-EVENT)) 

TOP-COLOR) ) 

(NEXT-COLOR  (IF  THIS-EVENT 

(COND  ( (AND  (FREE?  COLOR)  (START?  NEW-COLOR) )  PROJ-COIX>R) 

( (AND  (FP?  COLOR)  (OBST?  NEW-COLOR)  )  FREE-COLOR) 

(T  COLOR) ) 

TOP-COLOR) ) 

(PREV-COLOR  (IF  (AND  (FREE?  COLOR)  (START?  OLD-COLOR))  PROJ-CCLOR  COLOR))) 

(COND  (  (AND  PREV-EVENT 

(OR  (AND  (FP?  NEXT-COLOR) 

(PLUSP  (SLOPE->  (EQN  PREV-EVENT)  BOTrOM-TANGENT-EQH) ) ) 

(AND  (FP?  PREV-COLOR) 

(PLUSP  (SLOPE->  TOP-TANGENT-EQN  (EQN  PREV-EVENT) ) ) ) ) ) 

(SETF  (COLOR-CHANGE  PREV-EVENT)  (ADD-COLORS  (COLOR-CHANGE  PREV-EVENT) 

PROJ-COLOR) ) ) ) 

(COND  ((AND  (FREE?  PREV-COLOR) 

(FP?  NEXT -COLOR) 

(IN-BETWEEN  P)tEV-EVENT  BOTTOM-TANGENT-EQH  THIS-EVENT)) 

(cond  (  (conput«-bac)cpro  jaction-alidlnu  pr«v-«v«nt  top-tAnG«nt-«qn 


V.V.  J 


bottO!n-t«ng«nt-«qn  SA  'bottom) 


(SETF  (REST  PTR) 

(CONS  (MAKE-EVENT  EQN  BOTTOM-TANGENT-EQN  COLOR-CHANGE  PROJ-COLOR) 
(REST  PTR) ) ) 

(SETQ  PTR  (REST  PTR))) 

(prov-ovont 

(■•tf  (color-e)ian9«  prov-ovont)  (add-color*  (color-cbang*  pr*v-*v*nt) 

pro j-color) ) ) ) ) ) 

(CONO  (  (AND  (FREE?  NEXT-COLOR) 

(FP?  PREV-COLOR) 

(IN-BETNEXN  PREV-EVENT  TOP-TANGENT-EQN  THIS-EVENT) 

(computa-bae)cpro jaction-alldlng  this-avant  top-tangant-aqn 

'  bottom-tangant-aqn  SA  ())) 

(SETF  (REST  PTR)  (CONS  (MAKE-EVENT  EQN  TOP-TANGENT-EQN 

COLOR-CHANGE  (NEC-COLOR  PROJ-COLOR)  ) 

(REST  PTR) ) ) 

(SETQ  PTR  (REST  PTR) ) ) ) 

(SETQ  OLD-COLOR  COLOR) 

(SETQ  COLOR  NEH-COLOR) ) ) ) 

:  should  ratum  T  If  wa  should  aract  a  valocity  constraint 

(dafun  cacBputa-bac)cpre jaction-alldlng  (avant  top-tangant-agn  bottom-tangant-agn  SA  bottom?) 
(cond  ( (Not  (bae)cpro  jaetlon?) )  T) 

((null  avant)  T) 

(T  (lat  ( (adga-agn  (aqn  avant))) 

(cond  III  ((start?  (color-changa  avant))  nil) 

(T 

(salactq  (bae)(pro jaetlon?) 

(NaaJt 

(not  (aaa]c-ean-sllda?  (orlant-adga-aguatlon  adga-aqn  bottom?)))) 
(Strong 

(not  (snjst-sllda?  (orlant-adga-aguatlon  adga-agn  bottom?) )  )  ) 
(old-«*a)i  (not  (old-«aa)t-can-sllda?  adga-agn 

top-tangant-aqn 

bottost-tangant-aqn  SA) ))))))))) 


III  avant  normals  aro  orlantad  upuarda,  always.  Expact  for  vartleal  avanta. 
Ill  whara  th  normals  point  to  tha  laft. 


(dafun  oElant-adga-aquatlon  (aqn  bottom?) 

(whan  aqn 

(If  (or  (and  (sarop  (y  aqn)) 

(siinusp  (X  aqn) ) ) 

(and  bottom?  (sdnusp  (y  aqn) ) ) 

(and  (not  bottom?)  (plusp  (y  aqn) ) ) ) 

(nag-aqn  aqn) 
aqn) ) ) 

;  Dacldas  If,  using  t)M  OPPOSITE  valocity  cona,  wa  can  sllda  on  th* 
;  adg*  with  aqn  EOGE-EQN . 


(dafun  old-w*a](-can-allds?  (*dg*-*qn  top-tangant-aqn  bettosn-tangant-agn  sa) 

(and  adga-aqn  (can-sllda?  adga-agn  top-tangant-aqn  bottosi-tangant-aqn  SA) ) ) 

;  doaa  not  handl*  th*  eas*  whara  am  *  0  and  thara' a  no  valocity  uncertainty 

(dafun  w*a](-can-sllda?  (aqn  toptional  (n*g-vl  ■nag-vl*)  (n*g-v2  *n*g-v2*)  (mu  *mu*)) 
(and  aqn 

(lat*  ((normal  (list  (float  (x  aqn))  (float  (y  aqn)))) 

(tangant  (list  (*  sai  (cadr  normal))  (*  (minus  mu)  (ear  normal))))) 

(lat  ((*1  (ve2add  normal  tangent)) 

(*2  (vo-sub  nonsal  tangant))) 

(or  (•  SM  0.0) 

(not  (In-2D-eon*-p  nag-vl  *1  *2) ) 

(not  (In-2D-cona-p  n*g-v2  *1  *2))))))) 


(OEFUN  MUST-SLIDE?  (EQN) 
(and  aqn 

(slld*-p  aqn) ) ) 
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;  modlflad  to  cIom  In  cam  of  bockpro joctlon 


(OEFUN  UPDATE-HIDDLE-INTERVAL  (TOP-INTERVAL  MIDDLE-INTERVAL  BOTTOM- INTERVAL 

EVENTS  TOP -TANGENT-EON  BOTTOM-TANGENT -EQN  SA  POINT 
INT->-  INT->*  EVENT-QUEUE  8MEEP-LINE  POLYS  OLD-STRUCTS  QUERY) 
(COND  ((AND  (not  (bocJcpro joctlon?)  ) 

(FREE?  (COLOR  (POLYGON  (OOTPUT-STROCTURE  BOTTOM- INTERVAL)  )  )  ) 

(NOT  (START?  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  TOP-INTERVAL)  )  )  )  )  ) 
(MULTIPLE-VALUE  (MI DOLE -INTERVAL  SNEEP-LINE  EVENT-QUEUE  EVENTS  POLYS  OLD-STRUCTS) 
(CONTINUE-MIDDLE  MIDDLE -INTERVAL 

(IF  EVENTS  (EQN  (FIRST  EVENTS)  )  TOP-TANGENT-EQN) 

(IF  (AMO  EVENTS  (PLUSP  (SLOPE->  BOTTOM-TANGENT-EQN 

(EQN  (FIRST  EVENTS) ) ) ) ) 

(EON  (FIRST  EVENTS)) 

BOTTOM-TANGENT-EQN) 

TOP-TANGENT-EQN  BOTTOM-TANGENT-EQN  SA  POINT  IMT->-  INT->* 
SWEEP-LINE  EVENT-QUEUE  EVENTS  POLYS  OLD-STRUCTS  QUERY) ) ) 

((AND  (not  (bac)ipro  jactlon?) ) 

(FREE?  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  TOP-INTERVAL) ) ) ) 

(NOT  (START?  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  BOTTOM- INTERVAL)  )  )  )  )  ) 
(MULTIPLE-VALUE  (MIDDLE-INTERVAL  SWEEP-LINE  EVENT-QUEUE  EVENTS  POLYS  OLD-STRUCTS) 
(CONTINUE-MIOOLE  MIDDLE-INTERVAL 

(IF  (AND  EVENTS  (PLUSP  (SLOPE->  (EQN  (FIRST  (LAST  EVENTS)  )  ) 

TOP-TANGENT-EQN) ) ) 

(EQN.  (FIRST  (LAST  EVENTS)  )  ) 

TOP -TANGENT-EQN ) 

(IF  EVENTS  (EQN  (FIRST  (LAST  EVENTS) ) )  BOTTOM-TANGENT -EQN) 
TOP-TANGENT-EQN  BOTTOM-TANGENT-EQN  SA  POINT  INT->-  INT->+ 
SWEEP-LINE  EVENT-QUEUE  EVENTS  POLYS  OLD-STRUCTS  QUERY) ) ) 

(T  (MULTIPLE-VALUE  (POLYS  OLD-STRUCTS) 

(CLOSE-INTERVAL  MIDDLE-INTERVAL  POINT  POLYS  OLD-STRUCTS  QUERY)  ) 

(SETQ  SWEEP-LINE  (AVL-DELETE  MIDDLE-INTERVAL  SWEEP-LINE  INT->-  QUERY)  ) 

(SETQ  MIDDLE-INTERVAL  NIL) ) ) 

(COND  (  (AND  MIDDLE-INTERVAL  (FP?  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  BOTTOM- INTERVAL)  )  ) )  ) 
(MULTIPLE-VALUE  (SNEEP-LINE  EVENT-QUEUE  OLD-STRUCTS) 

(JOIN-INTERVALS  MIDDLE-INTERVAL  BOTTOM- INTERVAL 

POINT  INT->-  INT->+  SWEEP-LINE  EVENT-QUEUE  OLD-STRUCTS  QUERY))) 
((AND  MIDDLE-INTERVAL  (FP?  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  TOP-INTERVAL))))) 
(MULTIPLE-VALUE  (SWEEP-LINE  EVENT-QUEUE  OLD-STRUCTS) 

(JOIN- INTERVALS  TOP-INTERVAL  MIDDLE-INTERVAL 

POINT  INI->-  INT->*  SWEEP-UNE  EVENT-QUEUE  OLD-STRUCTS  QUERY)))) 
(VALUES  SWEEP-LINE  EVENT-QUEUE  EVENTS  POLYS  OLD-STRUCTS)) 


(DEFtni  CONTINUE-MIDDLE  (MIDDLE-INTERVAL  MIDDLE-TOP  MIDDLE-BOTTOM  TOP-TANGENT-EQN 

BOTTOM-TANGENT-EQN  SA  POINT  INT->-  INT->+ 

SWEEP-LINE  EVENT-QUEUE  EVENTS  POLYS  OLD-STRUCTS  QUERY) 

(COND  (  (AND  (EQUAL  MIDDLE-TOP  MIDDLE-BOTTOM) 

(NOT  (CAN-SLIDE?  MIDDLE-TOP  TOP-TANGENT-EQN  BOTTOM-TANGENT-EQN  SA) )  ) 
(MULTIPLE-VALUE  (POLYS  OLD-STRUCTS) 

(CLOSE-INTERVAL  MIDDLE-INTERVAL  POINT  POLYS  OLD-STRUCTS  QUERY)) 

(SETQ  SNEEP-LINE  (AVL-DELETE  MIDDLE-INTERVAL  SNEEP-LINE  INT->-  QUERY)  ) 

(SETQ  MIDDLE-INTERVAL  NIL)) 

(T  (SETQ  EVENTS  (MERGE-EVENTS  EVENTS  (LIST  (MAKE-EVENT  EQN  MIDDLE-BOTTOM 

COLOR-CHANGE  PROJ-COLOR) 
(MAKE-EVENT  EQN  MIDDLE-TOP 
COLOR-CHANGE 

(NEC-COLOR  PROJ-COLOR) ) ) ) ) 

(UNLESS  (EQUAL  MIDDLE-TOP  MIDDLE-BOTTOM) 

(COND  ( (PLUSP  (SLOPE->  MIDDLE-TOP  TOP-TANGENT-EQN) ) 

(SETQ  SWEEP-LINE  (CREATE-INTERVAL  MIDDLE-TOP  TOP-TANGENT-EQN  FREE-COLOR 

POINT  INI->f  SWEEP-LINE  QUERY)) 

(SETQ  MIDDLE-TOP  TOP-TANGENT-EQN))) 

(COND  ( (PLUSP  (SLOPE->  BOTTOM-TANGENT-EQN  MIDDLE-BOTTOM) ) 

(SETQ  SWEEP-LINE 

(CREATE-INTERVAL  BOTTC»<-TANGENT-EQN  MIDDLE-BOTTOM  FREE-COLOR 
POINT  IHT->+  SWEEP-LINE  QUERY) ) 

(SETQ  MIDDLE-BOTTOM  BOTTOM-TANGENT-EQN) ) ) ) 

(MULTIPLE-VALUE  (SNEEP-LINE  EVENT-QUEUE) 

(CHANGE-INTERVAL  MIDDLE-INTERVAL  MIDDLE-TOP  MIDDLE-BOTTOM 

POINT  INT->-  INT->*  SWEEP-LINE  EVENT-QUEUE  QUERY) ) ) ) 

(VALUES  MIDDLE-INTERVAL  SWEEP-LINE  EVENT-QUEUE  EVENTS  POLYS  OLD-STRUCTS)) 


(DEFUN  CAN-SLIDE?  (EQN  TOP-TANGENT-EQN  BOTTOM-TANGENT-EQN  SA) 
(>  (MAX  (ABS  (//  (DOT  EQN  TOP-TANGENT-EQN) 
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.-.V. 

'.v.v. 


.V. 


•  .** 


(SORT  (•  (DOT  EOH  EON>  (DOT  TOP-TANCEHT-EQN  TOP-TANGERT-EOII)  )  )  )  ) 

(ABS  (//  (DOT  EON  BOTTOM- TANGENT -CQH) 

(SORT  (•  (DOT  EON  EQN)  (DOT  BOTTOM-TANGENT-EQN  BOTTOM-TANGENT-EQN) ) ) 1 ) ) 


(OEFUN  CHANGE-INTERVAL  (INTERVAL  TOP  BOTTOM  POINT  INT->-  lNT->*  SWEEP-LINE  EVENT-OOEUE  0OE*Tf) 
(LET  ((STRUCT  (OUTPUT-STRUCTURE  INTERVAL))) 

(COND  ((NOT  (AMD  (EQUAL  (TOP  INTERVAL)  TOP) 

(EQUAL  (BOTTOM  INTERVAL)  BOTTOM))) 

(COND  ((NOT  (EQUAL  (TOP  INTERVAL)  TOP)) 

(SETT  (TAIL  (FIRST  (TOP  STRUCT)))  POINT) 

(SETF  (TOP  STRUCT)  (CONS  (MAKE-EDGE  HEAD  POINT  EQN  TOP)  (TOP  STRUCT))))) 
(COND  (  (MOT  (EQUAL  (BOTTOM  INTERVAL)  BOTTOM)  ) 

(SETT  (HEAD  (FIRST  (BOTTOM  STRUCT) ) )  POINT) 

(SETF  (REST  (BOTTOM  STRUCT)) 

(NCONS  (MAKE-EDGE  TAIL  POINT  EQN  (NEG-EQM  BOTTOM)  ) )  ) 

(SETF  (BOTTOM  STRUCT)  (REST  (BOTTOM  STRUCT) ) ) ) ) 

(LET  ( (MEW-INTERVAL  (MAKE-INTERVAL  TOP  TOP  BOTTOM  BOTTOM 

OUTPUT-STRUCTURE  STRUCT) ) ) 

(SETQ  SWEEP-LINE  (AVL-DELETE  INTERVAL  SWEEP-LINE  INT->-  QUERY) ) 

(SETQ  SWEEP-LINE  (AVL-INSERT  NEW-IMTERVAL  SWEEP-LINE  INT->-»  QUERY) ) 

(IF  (EVENT  INTERVAL) 

(SETQ  EVENT-QUEUE  (EVENT-SUB  (EVENT  INTERVAL)  EVENT-QUEUE) ) ) 

(SETQ  EVENT-QUEUE  (CLOSURE-EVENT?  NEW-INTERVAL  EVENT-QUEUE)  > ) )  )  ) 

(VALUES  SWEEP-LIME  EVENT-QUEUE)  ) 

(DEFUN  SPLIT-INTERVAL  (INTERVAL  TOP  BOTTOM  POINT  INT->-  IMT->4’  SWEEP-LINE  EVENT-QUEUE  QUERY) 
(LET*  ( (BOTTOM-STRUCT  (OUTPUT-STRUCTURE  INTERVAL)) 

(TOP-STRUCT  (ItAKE-OOTPUT-STRUCTURE  POLYGON  (POLYGON  BOTTOM-STRUCT) 

TOP  (TOP  BOTTOM-STRUCT) 

TOP-END  (TOP-END  BOTTOM-STRUCT) 

BOTTOM-END  BOTTOM-STRUCT))) 

(SETF  (BOTTOM-END  (TOP-END  BOTTOM-STRUCT))  TOP-STRUCT) 

(SETF  (TOP-END  BOTTOM-STRUCT)  TOP-STRUCT) 

(SETF  (TOP  BOTTOM-STRUCT)  (LIST  (MAKE-EDGE  HEAD  POINT  EON  BOTTOM) 

(MAKE-EDGE  TAIL  POINT  EQN  (NEG-EQM  TOP)  )  )  ) 

(SETF  (BOTTOM  TOP-STRUCT)  (REST  (TOP  BOTTOM-STRUCT)  )  ) 

(SETQ  SWEEP-LIME  (AVL-DELETE  INTERVAL  SWEEP-LIME  INT->-  QUERY) ) 

(IF  (EVENT  INTERVAL) 

(SETQ  EVENT-QUEUE  (EVENT-SUB  (EVENT  INTERVAL)  EVENT-QUEUE) ) ) 

(LET  ( (TOP-INTERVAL  (MAKE-INTERVAL  TOP  (TOP  INTERVAL)  BOTTOM  TOP 

OOIPOT-STRUCIOHE  TOP-STRUCT) > 

(BOTTOM-INTERVAL  (MAKE-INTERVAL  TOP  BOTTOM  BOTTOM  (BOTTOM  INTERVAL) 

OUTPUT-STRUCTURE  BOTTOM-STRUCT) ) ) 

(SETQ  SWEEP-LINE  (AVL-INSERT  BOTTOM-INTERVAL 

(AVL-INSERT  TOP-INTERVAL  SWEEP-LINE  INT->*  QUERY) 

INT->+  QUERY) ) 

(SETQ  EVENT-QUEUE  (CLOSURE -EVENT?  BOTTOM-INTERVAL 

(CLOSURE-EVENT?  TOP-INTERVAL  EVENT-QUEUE) ) ) ) ) 

(VALUES  SWEEP-LINE  EVENT-QUEUE) ) 

(DEFUN  JOIN-INTERVALS  (TOP-INTERVAL  BOTTOM- INTERVAL 

POINT  INT->-  INT->«  SWEEP-LINE  EVENT-QUEUE  OLD-STRUCTS  QUERY) 

(LET*  ( (TOP-STRUCT  (OUTPUT-STRUCTURE  TW-INTERVAL)  ) 

(BOTTOM-STRUCT  (OUTPUT -STRUCTURE  BOTTOM-INTERVAL)) 

(TOP-POLY  (POLYGON  TOP-STRUCT)) 

(BOTTOM-POLY  (POLYGON  BOTTOM-STRUCT))) 

(SETF  (HEAD  (FIRST  (BOTTOM  TOP-STRUCT)))  POINT) 

(SETF  (TAIL  (FIRST  (TOP  BOTTOM-STRUCT)))  POINT) 

(COND  ( (EQ  (TOP-END  BOTTOM-STRUCT)  TOP-STRUCT) 

(SETF  (LOOP-LIST  TOP-POLY) 

(CONS  (TOP  BOTTOM-STRUCT)  (LOOP-LIST  TOP-POLY))) 

(IF  (NULL  (LOOP-END  TOP-POLY)) 

(SETF  (LOOP-END  TOP-POLY)  (LOOP-LIST  TOP -POLY) ) ) 

(SETF  (LOOP-LIST  BOTTOM-POLY) 

(LOOP-UST  TOP-POLY)) 

(SETT  (LOOP -END  BOTTOM-POLY)  (LOOP-END  TOP-POLY) ) ) 

(T  (SETF  (REST  (BOTTOM  TOP-STRUCT))  (TOP  BOTTOM-STRUCT)) 

(SETF  (TOP-END  (BOTTOM-END  TOP-STRUCT) )  (TOP-END  BOTTOM-STRUCT) ) 

(SETF  (BOTTOM-END  (TOP-END  BOTTOM-STRUCT))  (BOTTOM-END  TOP-STRUCT)))) 

(SETF  (BOTTOM-END  (TOP-END  TOP-STRUCT))  BOTTOM-STRUCT) 

(SETF  (TOP  BOTTOM-STRUCT)  (TOP  TOP-STRUCT)) 

(SETF  (TOP-END  BOTTOM-STRUCT)  (TOP-END  TOP-STRUCT)) 


(COND  (QUERY 

(SETT  (BOTTOM-EHD  TOP-ST»UCT>  BOTTOM-STRUCT) 

(SETT  (POLYGOW  TOP-STRUCT)  'DISCARDED) 

(SETQ  OLD-STRUCTS  (CONS  TOP-STRUCT  OLD-STRUCTS) ) ) ) 

(COND  ( (NOT  (EQ  (LOOP-LIST  TOP-POLY) 

(LOOP -LI ST  BOTTOM-POLY) ) ) 

(IF  (LOOP-LIST  BOTTOM-POLY) 

(SETT  (REST  (LOOP-END  BOTTOM-POLY) ) 

(LOOP-LIST  TOP-POLY) ) 

(SETT  (LOOP-LIST  BOTTOM-POLY)  (LOOP-LIST  TOP-POLY) ) ) 

(IF  (LOOP-END  TOP-POLY) 

(SETF  (LOOP-END  BOTTOM-POLY)  (LOOP-END  TOP-POLY))) 

(SETF  (LOOP-LIST  TOP-POLY)  (LOOP-LIST  BOTTOM-POLY)) 

(SETF  (LOOP-END  TOP-POLY)  (LOOP-END  BOTTOM-POLY) ) ) ) 

(SETQ  SWEEP-LINE  (AVL-DELETE  TOP-INTERVAL 

(AVL-DELETE  BOTTOM-INTERVAL  SWEEP-LINE  INT->-  QUERY) 
INT->-  QUERY) ) 

(IF  (EVENT  TOP-INTERVAL) 

(SETQ  EVENT-QUEUE  (EVENT-SUB  (EVENT  TOP-INTERVAL)  EVENT-QUEUE) ) ) 

(IF  (EVENT  BOTTOM- INTERVAL) 

(SETQ  EVENT-QUEUE  (EVENT-SUB  (EVENT  BOTTOM- INTERVAL)  EVENT-QUEUE)  )  ) 

(LET  ((INTERVAL  (MAKE -INTERVAL  TOP  (TOP  TOP-INTERVAL)  BOTTOM  (BOTTOM  BOTTOM- INTERVAL) 

OUTPUT-STRUCTURE  BOTTOM-STRUCT) ) ) 

(SETQ  SWEEP-LINE  (AVL- INSERT  INTERVAL  SWEEP-LINE  INT->»  QUERY)  ) 

(SETQ  EVENT-QUEUE  (CLOSURE-EVENT?  INTERVAL  EVENT-QUEUE)))) 

(VALUES  SWEEP-LINE  EVENT-QUEUE  OLD-STRUCTS) ) 


(DEFUN  CLOSURE-EVENT?  (INTERVAL  EVENT -QUEUE) 

(LET  ((MEW-POINT  (INTERSECTION-POINT  (TOP  INTERVAL)  (BOTTOM  INTERVAL)))) 
(COND  (NEW-POINT 

(SETF  (EVENT  INTERVAL) 

(MAKE-EVENT  POINT  NEW-POINT  COLOR-CHANGE  1) ) 

(SETQ  EVENT-QUEUE  (EVENT-ADD  (EVENT  INTERVAL)  EVENT-QUEUE)  )  )  )  ) 
EVENT-QUEUE) 


(OEFUN  NEW-IMTERVALS  (INTERVALS  EVENTS  POINT  INT->»  SWEEP-LINE  QUERY) 

(DO  ( (PTR  EVENTS  (REST  PTR) ) 

(COLOR  (COLOR  (POLYGON  (OUTPUT-STRUCTURE  (FIRST  INTERVALS) ) ) ) ) ) 

( (NULL  (REST  PTR) I  SWEEP-LIME) 

(LET  ((TOP-EVENT  (SECOND  PTR) ) 

(BOTTOM-EVENT  (FIRST  PTR) ) ) 

(SETQ  COLOR  (ADD-COLORS-CARD'ULLY  COLOR  (COLOR-CHANGE  BOTTOM-EVENT)  )  ) 

(SETQ  SWEEP-UNE  ( CREATE- 1 NTWVAL  (EQN  TOP-EVENT)  (EQN  BOTTOM-EVENT)  COLOR  POINT 

IHT->*  SWEEP-LINE  QUERY))))) 


(DEFUN  CREATE-INTERVAL  (TOP  BOTTOM  COLOR  POINT  INT->*  SWEEP-LINE  QUERY) 

(LET  ((STRUCT  (MAKE -OUTPUT-STRUCTURE 

POLYGON  (MAKE-POLYGON  COLOR  COLOR) 

TOP  (LIST  (MAKE-EDGE  HEAD  POINT  EQN  TOP) 

(MAKE-EDGE  TAIL  POINT  EQN  (NEG-EQN  BOTTOM)  )  )  )  )  ) 
(SETF  (BOTTOM  STRUCT)  (REST  (TOP  STRUCT))) 

(SETT  (BOTTOM-Em)  STRUCT)  STRUCT) 

(SETF  (TOP-END  STRUCT)  STRUCT) 

(AVL-INSERT  (MAKE-I?»TERVAL  TOP  TOP  BOTTOM  BOTTOM  OUTPUT-STRUCTURE  STRUCT) 
SWEEP-LINE  IHT->'»  QUERY)  )  ) 

(DEFUN  FIX -OLD-STRUCTS  (OLD-STRUCTS) 

(DOLIST  (STRUCT  OLD-STRUCTS) 

(LET  ((POLY  (DO  ((PTR  STRUCT  (BOTTOM-END  PTR))) 

((NOT  (EQ  'DISCARDED  (POLYGON  PTR)))  (POLYGON  PTR))))) 

(DO  ((PTR  STRUCT  (BOTTOM-END  PTR))) 

((NOT  (EQ  'DISCARDED  (POLYGON  PTR)))) 

(SETF  (POLYGON  PTR)  POLY) ) ) ) ) 

(DEFUN  PROBLEM-SIZE  (PROBLEM) 

IlS.T  (  (SIZE  0)  ) 

(COND  ( (EOCE-P  PROBLEM)  1) 

(  (POLYGON-P  PROBLEM) 

(DOLIST  (LOOP  (LOOP-LIST  PROBLEM)) 

(SETQ  SIZE  (*  SIZE  (PROBLEM-SIZE  LOOP) ) ) ) 

SIZE) 

( (LISTP  PROBLEM) 

(DOLIST  (PROB  PROBLEM) 


1  -mj'  ■%: 


tf. 


(SCTQ  SIZE  («  SIZE  (PROBLEM-SIZE  PROB) ) 1) 
SIZE) ) ) ) 


;;  Sliding  pradlcac*  from  Mika  Erdmann. 


rhi*  function  dacldaa  Mhsthar  non#  of  tha  valoclcy  vactora  in  cha  ranga 
through  v2  cauaa  acicklng  on  an  adga  apaclflad  by  'normal*. 


(dafun  alida-p  (aqn  ^optional  (nag-vl  ‘nag-vl*)  (nag-v2  •nag-v2*)  (mu  •mu*)) 
(lat*  ((normal  (liat  (float  (x  aqn))  (float  (y  aqn)))) 

(tangant  (liat  (*  mu  (cadr  normal))  (*  (atlnua  mu)  (car  normal))))) 
(lat  ( (al  (vc2add  normal  tangant)) 

(a2  (vc-aub  normal  tangant))) 

(print-lina  (liat  tangant  al  a2) ) 

(lat  ((II  (not  (In-2D-cona-p  normal  nag-vl  nag-v2) ) ) 

(12  (not  (In-2D-eona-p  nag-vl  al  a2))) 

(13  (not  (In-20-oona-p  nag-v2  al  a2)))) 

(prlnt-llna  (Hat  II  12  13)) 

(and  (not  (In-2D-eona-p  normal  nag-vl  nag-v2)) 

(or  (-  mi  0) 

(and  (not  (In-20-eona-p  nag-vl  al  a2) ) 

(not  (In-2D-cona-p  nag-v2  al  a2)))))))) 


Givan  that  tha  two  2-0  vactora  al  and  a2  ara  Indapandant, 
thia  function  dacldaa  whathar  a  third  2-0  vactor,  v, 
i.iaa  In  tha  cona  formad  by  tha  poaltlva  apan  of  al  and  a2. 


(dafun  Old-In-20-eona-p  (v  al  a2) 

(lat  ( (parp  (20-erosa-produet  al  a2)) 
(al-x-v  (20-eroaa-produet  al  v) ) 
(v-x-a2  (20-eroas-produet  v  a2) ) ) 
(and  (>•  (*  al-x-v  parp)  0) 

(>-  (•  v-x-a2  parp)  0)))) 


!!i  thla  la  bacauaa  tha  aultlpllcatlon  can  ovarflow. 


(dafun  In-2D-cona-p  (v  al  a2) 

(lat  ((parp  (2D-crosa-product  al  a2) ) 

(al-x-v  (20-crosa-produet  al  v) ) 

(v-x-a2  (20-croaa-produot  v  a2))> 

(lat  ( (algparp  (algnum  parp))) 

(and  (>-  (•  (algnum  al-x-v)  algparp)  0) 

(>•  (•  (algnum  v-x-a2)  algparp)  0) ) > ) ) 


j ;;;;;;;;;;;  j X  /»  j 

Thla  function  computaa  tha  croaa-product  of  Ita  two  2-0  vactor  argumanta. 


r ;;  j X I »;  j »;;;»»»»);»» J  X ;);)) i 


:  X  X ; ; ;  X ;  x ;  X ;  X ; ; ;  X  X  x  X  X 


(dafun  2D-croaa-product  (vl  v2) 
(lat  ( (xl  (car  vl) ) 

(yl  (cadr  vl)) 

(x2  (car  v2) ) 

(y2  (cadr  v2)  )  ) 

(-  (•  xl  y2)  (•  x2  yl))  )  ) 


(dafun  vc2add  (vl  v2) 
(cond  ((or  (atom  vl) 


;  X  X  X ;  X  X  X  X  X  X  X  X ;  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X ; 


r  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X 


This  function  axtpacta  two  llata,  ropraaantlng  vactor. 
It  computaa  thalr  vactor  aum. 


r  X  X  X  X  X  X  X  X  X  X  X  X  X ;  X  X  X  X  X ; 


(  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X  X 
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•V."  ^  ‘a*  ‘  *• 


(atom  v2) )  (farror  'vactor  may  not  ba  an  atom  in  vcZadd*) ) 
It  (mapcar  (function  *)  vl  v2)))) 


thla  function  axpacta  two  llata.  rapraaantlnq  vactor 
It  coa^utaa  thalr  vactor  dlffaranca. 


(dafun  ve-aub  (vl  v2) 

(cond  ( (or  (atom  vl) 

(atom  v2) )  (farror  "vactor  may  not  ba  an  atom  In  vc-sub*) ; 
(t  (BMpcar  (function  -)  vl  v2)))) 


f  t :  f  t  f  f  t  f  1 1 


b«««tlO;  pAckAQ*t«w*«p/  ne4*tXi«p 


r  • 

r. 

L"  • 

K. 


V. 


;  Cod*  to  build  «nd  Mlntaln  AVL  tr***,  (c)  19S€  Rog*r-th*-AVL-ahrubb*r.  (*  ahrubbor)  . 


<DErSTRt;CT  (AVL-NODE  I  NAMED 

: PREDICATE 

(:PRINT  •<-*  -a  >*>“  (LEFT  AVL-NODE)  (VALUE  AVL-NODE)  (RIGHT  AVL-NQDE)  )  ) 

VALUE 

LEFT 

RIGHT 

BALANCE) 

(OEFUN  AVL-COPY  (TREE) 

(IF  (NULL  TREE) 

NIL 

(MAKE-AVL-NOOE  BALANCE  (BALANCE  TREE)  VALUE  (VALUE  TREE) 

LEFT  (AVL-COPY  (LEFT  TREE)) 

RIGHT  (AVL-COPY  (RIGHT  TREE) ) ) ) ) 


(DEFUN  AVL-SIZE  (TREE) 

(IF  (NULL  TREE) 

0 

a*  (■»  (AVL-SIZE  (LEFT  TREE)) 

(AVL-SIZE  (RIGHT  TREE)))))) 


;  Thia  d*t*rmin*a  tb*  )>*igtat  of  an  AVL  tr**  and  alao  e)>*c)ta  If  your  tr**  la 
;  out  of  balanc*  or  ‘Koyaanlaquatai*  In  Hopl  Indian.  Actual  holght  dlffaranc* 

:  ahould  b*  th*  aam*  aa  th*  balanc*  valu*.  and  a)>ould  b*  in  th*  ran^*  (-1,0,1). 


(DEFUN  AVL-HEIGHT  (TREE) 

(CONO  ((NULL  TREE)  0) 

(T  (LET  ((HL  (AVL-HEIGHT  (LEFT  TREE))) 

(HR  (AVL-HEIGHT  (RIGHT  TREE)))) 

(CONO  ((#  (-HR  HL)  (BALANCE  TREE)) 

(FORMAT  T  * 

T)i«  actual  ))*ight  diff*r*ne*  -a  do*a  not  mgrtm  with  t))*  balanc*  *ntry  -a  for  nod*  -a* 

(-  HR  HL)  (BALANCE  TREE)  TREE) ) 

((>  (ABS  (BALANCE  TREE))  1) 

(FORMAT  T  • 

Nod*  -a  ia  Koyaaniaquatai,  its  balanc*  valu*  la  -a*  TREE  (BALANCE  TREE)))) 

<1*  (MAX  HL  HR)))))) 


;  Function  to  find  an  entry  in  an  AVL  tr**.  PREDICATE  ahould  return  0  if  KEY  and 
;  th*  nod*  value  are  th*  aaaa,  '»'l  if  KEY  la  greater  than  tb*  nod*  valu*.  -1  ot)>*rwia*. 


(OEFUN  AVL-ACCESS  (KEX  TREE  PREDICATE) 

(AND  TREE 

(LET  ( (DIF  (FUNCALL  PREDICATE  KEY  (VALUE  TREE) ) ) ) 

(COND  ((PLUS?  OIF)  (AVL-ACCESS  KEY  (RIGHT  TREE)  PREDICATE)) 
( (MINUS?  OIF)  (AVL-ACCESS  KEY  (LEFT  TREE)  PREDICATE) ) 
(T  (VALUE  TREE)))))) 


;  Function  to  find  all  tb*  aatrl**  in  an  AVL  tr**  that  ar*  equal  to  KEY  according  to 
;  PREDICATE.  PREDICATE  ahould  r*tum  0  if  KEY  and  th*  nod*  valu*  ar*  th*  aaaM, 

;  '^1  If  KEY  la  greater  than  t))*  node  valu*,  -1  otherwla*. 


(DEFUN  AVL-ACCESS-ALL  (KEY  TREE  PREDICATE) 

(AND  TREE 

(LET  ( (DIF  (FUNCALL  PREDICATE  KEY  (VALUE  TREE) ) ) ) 

(COND  ( (PLUS?  DIF)  (AVL-ACCESS-ALL  KEY  (RIGHT  TREE)  PREDICATE) > 

( (MINUSP  OIF)  (AVL-ACCESS-ALL  KEY  (LEFT  TREE)  PREDICATE) ) 

(T  (NCONC  (AVL-ACCESS-ALL  KEY  (LEFT  TREE)  PREDICATE) 

(NCONS  (VALUE  TREE)) 

(AVL-ACCESS-ALL  KEY  (RIGHT  TREE)  PREDICATE) )))))) 
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Racurn  tha  laft-moat  valua  In  an  AVt  traa 


(DEFUN  LETT-MOST  (TREE) 

(COND  (  (NULL  TREE)  NIL) 

((NULL  (LETT  TREE))  (VALUE  TREE)) 
(T  (LETT-MOST  (LETT  TREE))))) 


;  Racurn  tha  rlght-Boat  valua  In  an  AVL  tcaa. 


(DEFUN  RIGHT-MOST  (TREE) 

(CONO  ((NULL  TREE)  MIL) 

(  (NULL  (RIGHT  TREE)  )  (VALUE  TREE)  ) 
(T  (RIGHT-MOST  (RIGHT  TREE) ) ) ) ) 


;  Racurn  Cha  auccaaaor  of  a  glvan  noda  in  an  AVL  traa.  Thla  racurna  tha  laft-noat 
noda  In  tha  traa  whoaa  valua  la  graatar  than  tha  valua  of  KEY. 


(OEFUN  AVL-WEXT  (KEY  TREE  PREDICATE) 

(AND  TREE 

(CONO  (  (MINUSP  (TUNCALL  PREDICATE  KEY  (VALUE  TREE)  )  ) 
(OR  (AVL-NEXT  KEY  (LEFT  TREE)  PREOICUITE) 
(VALUE  TREE) ) ) 

(T  (AVL-NEXT  KEY  (RIGHT  TREE)  PREDICATE)  ) )  )  ) 


(DEFUN  AVL-PREV  (KEY  TREE  PREDICATE) 

(AND  TREE 

(COMO  (  (PLUS?  (TUNCALL  PREDICATE  KEY  (VALUE  TREE)  )  ) 
(OR  (AVL-PREV  KEY  (RIGHT  TREE)  PREDICATE) 
(VALUE  TREE))) 

(T  (AVL-PREV  KEY  (LETT  TREE)  PREDICATE) ) ) ) ) 


;  Tha  first  Intaraatlnq  oparatlen  on  AVL  traaa.  This  Inaarta  THING  Into  tha  traa 
;  and  raturns  a  naw  traa  and  an  Intagar  which  la  tha  ehanga  In  halght  of  tha  traa. 

;  If  NO-mutATIOM  la  T.  tha  old  traa  la  net  chanQad,  but  a  naw  traa  la  ratumad 
;  centalnln«  tha  Inaartad  alaaant.  Thla  adds  only  about  (AVL-HEICHT  TREE)  antra  nedaa, 
1  l.a.  lo9(traa-alsa)  antra  apaea. 


(DCFUN  AVL-INSERT  (THING  TREE  PREDICATE  (OPTIONAL  NO-MUTATION) 

(IF  (NULL  TREE) 

(VALUES  (HAKE-AVL-NOOE  BALANCE  0  VALUE  THING)  1) 

(LET  ((DIF  (TUNCALL  PREDICATE  THING  (VALUE  TREE)))) 

(CONO  (  (ZERO?  DIT) 

(VALUES  (UPDATE-NODE  TREE  (BALANCE  TREE)  THING 

(LETT  TREE)  (RIGHT  TREE)  NO-MUTATION) 

0)  ) 

(  (PLUSP  DIT) 

(MULTIPLE-VALUE-BIND  (SUBTREE  HEIGHT-CHANGE) 

(AVL-INSERT  THING  (RIGHT  TREE)  PREDICATE  NO-MUTATION) 

(SETO  TREE  (UPDATE-NODE  TREE  (♦  (BALANCE  TREE)  HEIGHT-CHANGE) 

(VALUE  TREE)  (LETT  TREE)  SUBTREE  NO-MUTATION)  ) 

(IF  (>  (BALANCE  TREE)  1) 

(BALAHCE-RIOBT  TREE  1  NO-MUTATION) 

(VALUES  TREE  (IP  (PLUSP  (BALANCE  TREE) )  HEIGHT-CHANGE  0) ) ) ) ) 

(T  (MULTIPLE-VALUE-BIND  (SUBTREE  HEIGHT -CHANGE) 

(AVL-INSERT  THING  (LETT  TREE)  PREDICATE  NO-MUTATIOM) 

(SETQ  TREE  (UPOATE-NODE  TREE  (-  (BALANCE  TREE)  HEIGHT-CHANGE) 

(VALUE  TREE)  SUBTREE  (RIGHT  TREE)  NO-MUTATIOM)  ) 
(IF  (<  (BALANCE  TREE)  -I) 

(BALANCE-LEFT  TREE  1  NO-MUTATION) 

(VALUES  TREE  (IF  (MINUSP  (BALANCE  TREE))  HEIGHT-CHANGE  0))))))))) 


;  This  dalatas  an  antry  froa  an  AVL  traa. 

;  Thla  also  has  a  nun-nutatlnG  aoda  for  produclno  coharant  structures. 


(DEFUN  AVL-DELETE  (THING  TREE  PREDICATE  tOPTIONAL  NO-MUTATION) 
(IF  (NULL  TREE) 


(VALUES  MIL  0) 

(LET  ((DIF  (FUMCALL  PREDICATE  THING  (VALUE  TREE)))) 

(COND  ( (ZEROP  OIF) 

(ERASE-NODE  TREE  NO-MUTATION)) 

( (PLUSP  DIF) 

(MULTIPLE-VALUE-BIND  (SUBTREE  HEIGHT-CHANCRE) 

(AVL-DELETE  THING  (RIGHT  TREE)  PREDICATE  NO-MUTATION) 

(SETQ  TREE  (UPDATE-MODE  TREE  i*  (BALANCE  TREE)  HEIGHT -CHANGE) 

(VALUE  TREE)  (LEFT  TREE)  SUBTREE  NO-MUTATION) ) 

(IF  (<  (BALANCE  TREE)  -1) 

(BALANCE-LEFT  TREE  0  NO-MUTATION) 

(VALUES  TREE  (IF  (ZEROP  (BALANCE  TREE))  HEIGHT-CniANQE  0))))) 

(T  (MULTIPLE-VALUE-BIND  (SUBTREE  HEIGHT -CHANGE) 

(AVL-DELETE  THING  (LEFT  TREE)  PREDICATE  NO-MUTATION) 

(SETQ  TREE  (UPDATE-NODE  TREE  (-  (BALANCE  TREE)  HEIGHT-CHANGE) 

(VALUE  TREE)  SUBTREE  (RIGHT  TREE)  NO-MUTATION)  ) 

(IF  (>  (BALANCE  TREE)  1) 

(BALANCE-RIGHT  TREE  0  NO-KUTATION) 

(VALUES  TREE  (IF  (ZEROP  (BALANCE  TREE))  HEIGHT-CHANCX  0))))))))) 


;  Thla  gmf  rid  ot  •  value  that  haa  bean  found  In  tho  troo.  NODE  la  tbo  node  containing 
;  tha  value.  If  the  right  aubtraa  of  NODE  ia  hlgbar  than  Ita  left,  raplaoa  the  value 
:  of  NODE  with  the  value  of  the  left-anat  leaf  of  tha  right  aubtraa,  and  roB»ve  thla 
•'  iaaf  fro*  tha  right  aubtraa.  Otbarwlaa  raplaoa  NODE' a  value  with  tha  value  of  tha 
:  rlght-noat  leaf  of  tha  left  aubtraa  of  NODE,  and  raawva  thla  leaf  fro*  t>ia  laft  aubtraa. 


(DEFUN  ERASE-MODE  (NODE  HO-HUTATION) 

(COND  ((AND  (NULL  (LEFT  NODE))  (NULL  (RIGHT  NODE))) 

(VALUES  NIL  -1)) 

( (PLUSP  (BALANCE  NODE) ) 

(MULTIPLE-VALUE-BIND  (VAL  SUBTREE  HEIGHT-CHANGE) 

(AVL-HEAO  (RIGHT  NODE)  NO-HUTATION) 

(SETQ  MODE  (UPDATE-MODE  MODE  (*  (BALANCE  NODE)  HEIGHT-CHANGE) 

VAL  (LEFT  NODE)  SUBTREE  NO-MUTATION) ) 

(VALUES  NODE  HEIGRT-CHAMGE ) ) ) 

(T  (MULTIPLE-VALUE-BIND  (VAL  SUBTREE  HEIGHT-CHANGE) 

(AVL-TAIL  (LEFT  MODE)  NO-MUTATION) 

(SETQ  NODE  (UPOATE-NOOE  NODE  <-  (BALANCE  KODCI  HEIGHT-CHANGE) 

VAL  SUBTREE  (RIGHT  NODE)  NO-MUTATIOM) ) 

(VALUES  NODE  (IF  (ZEROP  (BALANCE  NODE) )  HEIGHT-CHANGE  0) ) ) ) ) ) 

>'  Thla  raturna  tha  head  (laftMat  alaaatnt)  in  tha  tree,  and  raanvaa  it  frOB  tha  tree. 
;  Uaaful  for  iaplaaonting  priority  quauea  aa  AVL  traaa. 

:  V'aluaa  ratumad  are  tha  value  of  tha  laftnoat  alaaiant,  the  awdlfiad  tree,  and  tha 
;  change  in  height  of  tho  tree. 


(DCFUN  AVL-HEAD  (TREE  (OPTIONAL  MO-HUTATION) 

(COND  ((NULL  TREE)  NIL) 

((NULL  (LEFT  TREE)) 

(VALUES  (VALUE  TREE)  (RIGHT  TREE)  -1)) 

(T  (MULTIPLE-VALUE-BIND  (HEAD-VALUE  SUBTREE  HEIGHT-CHANGE) 

(AVL-HEAD  (LEFT  TREE)  NO-MUTATION) 

(SETQ  TREE  (UPDATE-MODE  TREE  (-  (BALANCE  TREE)  HEIGHT-CHANGE) 

(VALUE  TREE)  SUBTREE  (RIGHT  TREE)  NO-MUTATICBI)  > 

(IF  (>  (BALANCE  TREE)  1) 

(MULTIPLE-VALUE  (TREE  HEIGHT -CHANGE) 

(BALANCE-RZOBT  TREE  0  NO-HUTATION)) 

(IF  (HOT  (ZEROP  (BALANCE  TREE)))  (SETQ  HEIGHT-CHANGE  0))) 

(VALDES  HEAD-VALUE  TREE  HEIGHT-CHANGE) ) ) ) ) 


;  This  raturna  t)>o  tall  (rightaoat  alaaiant)  in  tha  tree,  and  raawvaa  it  from  tha  tree. 
;  Valuaa  ratumad  are  tba  value  of  tba  rigbtiaoat  alenanc,  tho  modified  tree,  and  tha 
;  change  in  height  of  tha  tree. 


(DEFUN  AVL-TAIL  (TREE  (OPTIONAL  NO-MUTATION) 

(COND  ((NULL  TREE)  MIL) 

( (NULL  (RIGHT  TREE) ) 

(VALUES  (VALUE  TREE)  (LEFT  TREE)  -1) ) 

(T  (MULTIPLE-VALUE-BIND  (TAIL-VALUE  SUBTREE  HEIGHT-CHANGE) 
(AVL-TAIL  (RIGHT  TREE)  MO-MUTAT.ON) 


(SETQ  TREE  (UPDATE-NODE  TREE  (♦  (BALANCE  TREE)  HEIGHT-CHANGE) 

(VALUE  TREE)  (LE5~r  TREE)  SUBTREE  NO-KUTATION)  ) 
(IF  (<  (BALANCE  TREE)  -1) 

(MULTIPLE-VALUE  (TREE  HEIGHT -CHANGE) 

OSAIANCE-LEFT  TREE  0  NO-MUTATION)  1 
(IF  (NOT  (ZEROP  (BALANCE  TREE)))  (SETQ  HEIGHT-CHANGE  0))) 

(VALUES  TAIL-VALUE  TREE  HEIGHT-CHANGE))))) 


B«ianca  •  TREE  that  is  right -Koyaanlaouatal ,  l.a.  tha  right  subtraa  la  2  lavala 
highar  than  tha  laft  aubtraa.  HEIGHT-CHANGE  la  tha  halght  of  TREE  ralatlva  to  ita 
valua  bafora  tha  dalata/lnaart  oparatlor..  Balanca-right  ratuma  a  noda  and  tha  halght 
of  that  noda  ralatlva  to  tha  original  halght  of  TREE. 


EFUN  BALANCE-RIGHT  (TREE  HEIGHT-CHANGE  NO-MUT) 

(LET  (  (R  (RIGHT  TREE))) 

(COND  (  (PLUSP  (BALANCE  R)  ) 

(SETQ  TREE  (UPDATE-NODE  TREE  0  (VALUE  TREE)  (LEFT  TREE)  (LEFT  R)  NO-MUT) ) 
(SETQ  R  (UPDATE-NODE  R  0  (VALUE  R)  TREE  (RIGHT  R)  NO-MUT) ) 

(VALUES  R  (I-  HEIGHT-CHANGE))) 

((ZEROP  (BALANCE  R)  ) 

(SETQ  TREE  (UPDATE-NODE  TREE  1  (VALUE  TREE)  (LEFT  TREE)  (LEFT  R)  NO-MUT)  ) 
(SETQ  R  (UPDATE-NODE  R  -1  (VALUE  R)  TREE  (RIGHT  R)  NO-MUT)) 

(VALUES  R  HEIGHT-CHANGE) ) 

(T  (LET  (  (LR  (LEFT  R)  )  ) 

(SETQ  R  (UPDATE-NODE  R  (IF  (MINUSP  (BALANCE  LR) )  10) 

(VjlLUE  R)  (RIGHT  LR)  (RIGHT  R)  N(0-MUT)  ) 

(SETQ  TREE  (UPDATE-NODE  TREE  (IF  (PLUSP  (BALANCE  LR) )  -I  0) 

(VALUE  TREE)  (LEFT  TREE)  (LEFT  LR)  NO-MUT)  ' 
(SETQ  LR  (UPDATE-NODE  LR  0  (VALUE  LR)  TREE  R  NO-MUT)) 

(VALUES  LR  (1-  HEIGHT-CHANGE))))))) 


Balanca  a  TREE  that  la  laft-Koyaanlaguatsl,  l.a.  tha  laft  aubtraa  la  2  lavala 
highar  than  tha  right  aubtraa.  HEIGHT -CHANGE  la  tha  halght  of  TREE  ralatlva  to  Its 
valua  bafora  tha  dalata/lnaart  oparatlon.  Balanca-laft  raturns  a  noda  and  tha  halght 
of  that  noda  ralatlva  to  tha  original  halght  of  TREE. 


EFUN  BALANCE-LEFT  (TREE  HEIGHT -CHANGE  NO-MUT) 

(LET  ( (L  (LEFT  TREE) ) ) 

(COND  ((MINUSP  (BALANCE  L) ) 

(SETQ  TREE  (UPDATE-NODE  TREE  0  (VALUE  TREE)  (RIGHT  L)  (RIGHT  TREE)  NO-MUT)) 
(SETQ  L  (UPDATE-NODE  L  0  (VKLUt  L)  (LEFT  L)  TREE  NO-MUT)  ) 

(VALUES  L  (1-  HEIGHT -CHANGE) ) ) 

(  (ZEROP  (BALANCE  L)  ) 

(SETQ  TREE  (UPDATE-NODE  TREE  -1  (VALUE  TREE)  (RIGHT  L)  (RIGHT  TREE)  NO-MUT) ) 
(SETQ  L  (UPDATE-NODE  L  1  (VALUE  L)  (LEFT  L)  TREE  NO-MUT)) 

(VALUES  L  HEIGHT-CHANGE) ) 

(T  (LET  (  (RL  (RIGHT  L)  )  ) 

(SETQ  L  (UPDATE-NODE  L  (IF  (PLUSP  (BALANCE  RL)  )  -1  0) 

(VALUE  L)  (LEFT  L)  (LEFT  RL)  NO-MUT)  ) 

(SETQ  TREE  (UPDATE-NODE  TREE  (IF  (MINUSP  (BALANCE  RL) )  10) 

(VALUE  TREE)  (RIGHT  RL)  (RIGHT  TREE)  NO-MUT)  ) 
(SETQ  RL  (UPOATE-NOOE  RL  0  (VALUE  RL)  L  TREE  NO-MUT)  ) 

(VALUES  RL  (1-  HEIGHT-CHANGE))))))) 


Modify  an  axlstlng  AVL  noda  or  eroata  a  now  ona,  dapandlng  on  tha  valua  of  NO-MUTATION 


IF'JN  UPDATE-NODE  (NODE  BALANCE  VALUE  LEFT  RIGHT  NO-MUTATION) 

CF  NO-MUTATION 

(MAXE-AVL-NODE  BALANCE  BALANCE  VALUE  VALUE  LEFT  LEFT  RIGHT  RIGHT) 
(PROGN  (SETF  (BALANCE  NODE)  BALANCE) 

(SETT  (VALUE  NODE)  VALUE) 

(SETT  (LEFT  NODE)  LETT) 

(SETF  (RIGHT  NODE)  RIGHT) 

NODE) ) ) 


